(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    680265,      17552]
NotebookOptionsPosition[    659643,      16944]
NotebookOutlinePosition[    660062,      16960]
CellTagsIndexPosition[    660019,      16957]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell["2d kneed biped with torso", "Title",
 CellChangeTimes->{
  3.504390813710744*^9, {3.513011450427533*^9, 3.51301145527552*^9}, {
   3.513011745947039*^9, 3.513011749321201*^9}, {3.515858432663828*^9, 
   3.5158584360910234`*^9}, {3.5158700644438047`*^9, 
   3.5158700646998196`*^9}, {3.5158859650463057`*^9, 3.515885965342706*^9}}],

Cell[CellGroupData[{

Cell["initialization", "Section",
 CellChangeTimes->{
  3.504390830432091*^9, {3.513011606627004*^9, 3.513011611923624*^9}, {
   3.513011744096037*^9, 3.513011744363876*^9}, 3.514056528643134*^9}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"SetDirectory", "[", 
  RowBox[{"NotebookDirectory", "[", "]"}], "]"}], "\n", 
 RowBox[{
  RowBox[{"UtilPath", "=", 
   RowBox[{"FileNameJoin", "[", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"Directory", "[", "]"}], ",", "\"\<mathematica_util\>\""}], 
     "}"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"$Path", "=", 
   RowBox[{"DeleteDuplicates", "[", 
    RowBox[{"Append", "[", 
     RowBox[{"$Path", ",", "UtilPath"}], "]"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Needs", "[", "\"\<Units`\>\"", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Needs", "[", "\"\<RobotLinks`\>\"", "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Needs", "[", "\"\<ToMatlab`\>\"", "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Needs", "[", "\"\<ExtraUtil`\>\"", "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"Needs", "[", "\"\<ToCpp`\>\"", "]"}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"<<", "RobotLinks"}], ".", "m"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"<<", "Linearize"}], ".", "m"}], "*)"}]}], "\[IndentingNewLine]", 

 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"SetDirectory", "[", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], "<>", 
      "\"\<mathematica_util\>\""}], "]"}], ";"}], "*)"}]}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.541814047387916*^9, 3.541814047547925*^9}, 3.6192631624844837`*^9, {
   3.619263219508678*^9, 3.619263255612327*^9}, {3.619263306191246*^9, 
   3.6192633231676407`*^9}}],

Cell[BoxData["\<\"C:\\\\Users\\\\slongz\\\\Downloads\\\\proxi_case8-76\\\\\
dynamics_embedding\\\\mathematica_util\"\>"], "Output",
 CellChangeTimes->{{3.6192632560205717`*^9, 3.61926326470395*^9}, {
  3.619263308491706*^9, 3.6192633259481964`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Get", "::", "noopen"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Cannot open \[NoBreak]\\!\\(\\\"Linearize.m\\\"\\)\
\[NoBreak]. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/noopen\\\", ButtonNote -> \
\\\"Get::noopen\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.619263230444235*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"InertiaToCoriolis", "::", "shdw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Symbol \[NoBreak]\\!\\(\\\"InertiaToCoriolis\\\"\\)\
\[NoBreak] appears in multiple contexts \[NoBreak]\\!\\({\\\"RobotLinks`\\\", \
\\\"Global`\\\"}\\)\[NoBreak]; definitions in context \
\[NoBreak]\\!\\(\\\"RobotLinks`\\\"\\)\[NoBreak] may shadow or be shadowed by \
other definitions. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\
\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/shdw\\\", ButtonNote -> \
\\\"RobotLinks`InertiaToCoriolis::shdw\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.619263230412216*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"BodyJacobian", "::", "shdw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Symbol \[NoBreak]\\!\\(\\\"BodyJacobian\\\"\\)\[NoBreak] \
appears in multiple contexts \[NoBreak]\\!\\({\\\"RobotLinks`\\\", \
\\\"Global`\\\"}\\)\[NoBreak]; definitions in context \
\[NoBreak]\\!\\(\\\"RobotLinks`\\\"\\)\[NoBreak] may shadow or be shadowed by \
other definitions. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\
\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/shdw\\\", ButtonNote -> \
\\\"RobotLinks`BodyJacobian::shdw\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.619263230358184*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"ForwardKinematics", "::", "shdw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Symbol \[NoBreak]\\!\\(\\\"ForwardKinematics\\\"\\)\
\[NoBreak] appears in multiple contexts \[NoBreak]\\!\\({\\\"RobotLinks`\\\", \
\\\"Global`\\\"}\\)\[NoBreak]; definitions in context \
\[NoBreak]\\!\\(\\\"RobotLinks`\\\"\\)\[NoBreak] may shadow or be shadowed by \
other definitions. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\
\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/shdw\\\", ButtonNote -> \
\\\"RobotLinks`ForwardKinematics::shdw\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.6192632303041515`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"PrismaticTwist", "::", "shdw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Symbol \[NoBreak]\\!\\(\\\"PrismaticTwist\\\"\\)\[NoBreak] \
appears in multiple contexts \[NoBreak]\\!\\({\\\"RobotLinks`\\\", \
\\\"Global`\\\"}\\)\[NoBreak]; definitions in context \
\[NoBreak]\\!\\(\\\"RobotLinks`\\\"\\)\[NoBreak] may shadow or be shadowed by \
other definitions. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\
\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/shdw\\\", ButtonNote -> \
\\\"RobotLinks`PrismaticTwist::shdw\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.6192632302501187`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"RevoluteTwist", "::", "shdw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Symbol \[NoBreak]\\!\\(\\\"RevoluteTwist\\\"\\)\[NoBreak] \
appears in multiple contexts \[NoBreak]\\!\\({\\\"RobotLinks`\\\", \
\\\"Global`\\\"}\\)\[NoBreak]; definitions in context \
\[NoBreak]\\!\\(\\\"RobotLinks`\\\"\\)\[NoBreak] may shadow or be shadowed by \
other definitions. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\
\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/shdw\\\", ButtonNote -> \
\\\"RobotLinks`RevoluteTwist::shdw\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.6192632301970873`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"RigidPosition", "::", "shdw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Symbol \[NoBreak]\\!\\(\\\"RigidPosition\\\"\\)\[NoBreak] \
appears in multiple contexts \[NoBreak]\\!\\({\\\"Screws`\\\", \
\\\"Global`\\\"}\\)\[NoBreak]; definitions in context \
\[NoBreak]\\!\\(\\\"Screws`\\\"\\)\[NoBreak] may shadow or be shadowed by \
other definitions. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\
\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/shdw\\\", ButtonNote -> \
\\\"Screws`RigidPosition::shdw\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.619263230142054*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"RigidOrientation", "::", "shdw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Symbol \[NoBreak]\\!\\(\\\"RigidOrientation\\\"\\)\
\[NoBreak] appears in multiple contexts \[NoBreak]\\!\\({\\\"Screws`\\\", \
\\\"Global`\\\"}\\)\[NoBreak]; definitions in context \
\[NoBreak]\\!\\(\\\"Screws`\\\"\\)\[NoBreak] may shadow or be shadowed by \
other definitions. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\
\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/shdw\\\", ButtonNote -> \
\\\"Screws`RigidOrientation::shdw\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.6192632300880213`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"RPToHomogeneous", "::", "shdw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Symbol \
\[NoBreak]\\!\\(\\\"RPToHomogeneous\\\"\\)\[NoBreak] appears in multiple \
contexts \[NoBreak]\\!\\({\\\"Screws`\\\", \\\"Global`\\\"}\\)\[NoBreak]; \
definitions in context \[NoBreak]\\!\\(\\\"Screws`\\\"\\)\[NoBreak] may \
shadow or be shadowed by other definitions. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/shdw\\\", ButtonNote -> \
\\\"Screws`RPToHomogeneous::shdw\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.6192632300349903`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Get", "::", "noopen"}], "MessageName"], ":", 
  " ", "\<\"Cannot open \[NoBreak]\\!\\(\\\"RobotLinks.m\\\"\\)\[NoBreak]. \
\\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", \
ButtonFrame->None, ButtonData:>\\\"paclet:ref/message/General/noopen\\\", \
ButtonNote -> \\\"Get::noopen\\\"]\\)\"\>"}]], "Message", "MSG",
 GeneratedCell->False,
 CellAutoOverwrite->False,
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138643293`*^9}, {
  3.619263212349385*^9, 3.619263228861286*^9}}],

Cell[BoxData["$Failed"], "Output",
 CellChangeTimes->{{3.6192631037912893`*^9, 3.6192631138653297`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Get", "::", "noopen"}], "MessageName"], ":", 
  " ", "\<\"Cannot open \[NoBreak]\\!\\(\\\"Linearize.m\\\"\\)\[NoBreak]. \\!\
\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", \
ButtonFrame->None, ButtonData:>\\\"paclet:ref/message/General/noopen\\\", \
ButtonNote -> \\\"Get::noopen\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631138973494`*^9}}],

Cell[BoxData["$Failed"], "Output",
 CellChangeTimes->{{3.6192631037912893`*^9, 3.61926311389835*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"SetDirectory", "::", "cdir"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Cannot set current directory to \[NoBreak]\\!\\(\\\"C:\\\\\
\\\\Users\\\\\\\\slongz\\\\\\\\Downloads\\\\\\\\proxi_case8-76\\\\\\\\\
dynamics_embedding\\\\\\\\build_torso\\\"\\)\[NoBreak]. \\!\\(\\*ButtonBox[\\\
\"\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/cdir\\\", ButtonNote -> \
\\\"SetDirectory::cdir\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{{3.6192631037902884`*^9, 3.6192631139433765`*^9}}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"t0", "=", 
   RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5222466960362115`*^9, 3.5222467018365436`*^9}, {
  3.5425485959775953`*^9, 3.5425485986017456`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{"A_", "?", "MatrixQ"}], "]"}], ":=", 
  RowBox[{"ParallelTable", "[", 
   RowBox[{
    RowBox[{"Simplify", "[", 
     RowBox[{"A", "\[LeftDoubleBracket]", 
      RowBox[{"i", ",", "j"}], "\[RightDoubleBracket]"}], "]"}], ",", 
    RowBox[{"{", 
     RowBox[{"i", ",", 
      RowBox[{
       RowBox[{"Dimensions", "[", "A", "]"}], "\[LeftDoubleBracket]", "1", 
       "\[RightDoubleBracket]"}]}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{"j", ",", 
      RowBox[{
       RowBox[{"Dimensions", "[", "A", "]"}], "\[LeftDoubleBracket]", "2", 
       "\[RightDoubleBracket]"}]}], "}"}]}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{"A_", "?", "VectorQ"}], "]"}], ":=", 
  RowBox[{"ParallelTable", "[", 
   RowBox[{
    RowBox[{"Simplify", "[", 
     RowBox[{"A", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
     "]"}], ",", 
    RowBox[{"{", 
     RowBox[{"i", ",", 
      RowBox[{"Length", "[", "A", "]"}]}], "}"}]}], "]"}]}], "\n", 
 RowBox[{
  RowBox[{"ParallelSimplify", "[", "A_", "]"}], ":=", 
  RowBox[{"Simplify", "[", "A", "]"}]}]}], "Input",
 CellChangeTimes->{{3.525444031603923*^9, 3.525444057323394*^9}, 
   3.525444167371688*^9, {3.5254442077009945`*^9, 3.5254442142373686`*^9}, {
   3.5254442656763105`*^9, 3.52544431862834*^9}, {3.5254446389836626`*^9, 
   3.525444643198904*^9}, {3.5273464706540365`*^9, 3.527346473775215*^9}, 
   3.527423798626493*^9, {3.5387532837469378`*^9, 3.5387532943515444`*^9}, 
   3.54024855316775*^9}],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.547899799210141*^9, 3.5478998071545954`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["model specification", "Section",
 CellChangeTimes->{{3.513531803717296*^9, 3.513531806022168*^9}, {
  3.51767999292592*^9, 3.5176799960260973`*^9}, {3.5472307789416485`*^9, 
  3.5472307825248537`*^9}}],

Cell[CellGroupData[{

Cell["lt - length of toe", "Item",
 CellChangeTimes->{{3.5387467037455835`*^9, 3.5387467220756316`*^9}, {
  3.5387469219240627`*^9, 3.538746942720252*^9}}],

Cell["lh - length of heel", "Item",
 CellChangeTimes->{{3.538746943979324*^9, 3.5387469472625113`*^9}}],

Cell[TextData[{
 Cell[BoxData[
  FormBox["La", TraditionalForm]]],
 " - height of ankle"
}], "Item",
 CellChangeTimes->{{3.5387467037455835`*^9, 3.5387467220756316`*^9}, {
  3.5387469219240627`*^9, 3.5387469368939185`*^9}, {3.53874764902365*^9, 
  3.5387476510707674`*^9}, {3.538747723621917*^9, 3.5387477299472785`*^9}, {
  3.5425486017409253`*^9, 3.5425486029459944`*^9}}],

Cell["Lc - length of calf", "Item",
 CellChangeTimes->{
  3.538746724442767*^9, {3.5387469241501894`*^9, 3.538746965911578*^9}}],

Cell["Lt - length of thigh", "Item",
 CellChangeTimes->{{3.5387469662976007`*^9, 3.538746969660793*^9}}],

Cell["LT - Length of torso (in this model, set to distance to CoM)", "Item",
 CellChangeTimes->{{3.538747117996277*^9, 3.5387471264677615`*^9}, {
  3.5387471593016396`*^9, 3.5387471731104293`*^9}}],

Cell["rfx - distance from toe to ankle CoM", "Item",
 CellChangeTimes->{{3.5387470635941653`*^9, 3.5387470857944355`*^9}, 
   3.538747754905706*^9, {3.6062316350240393`*^9, 3.606231656434264*^9}}],

Cell["rfz - height from bottom of foot to ankle CoM", "Item",
 CellChangeTimes->{{3.5387469738520327`*^9, 3.53874700842901*^9}, {
   3.53874705405962*^9, 3.538747061914069*^9}, {3.5387477585549145`*^9, 
   3.53874775882493*^9}, 3.5473161868747025`*^9, {3.5473222394158883`*^9, 
   3.5473222447351923`*^9}, {3.606222004920229*^9, 3.606222016102869*^9}, 
   3.606231639005267*^9}],

Cell["rc - distance from ankle to calf CoM", "Item",
 CellChangeTimes->{{3.5387470771709423`*^9, 3.538747104217489*^9}, {
  3.6062220280775537`*^9, 3.606222054051039*^9}}],

Cell["rt - distance from knee to thigh CoM", "Item",
 CellChangeTimes->{{3.5387471048735266`*^9, 3.53874714130061*^9}, {
  3.6062220600123806`*^9, 3.60622206700578*^9}}],

Cell["rT - distance from hip to torso CoM (usually just set to 1)", "Item",
 CellChangeTimes->{{3.5387471416626306`*^9, 3.538747157245522*^9}, {
  3.6062220724660926`*^9, 3.6062220809465775`*^9}}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{"Needs", "[", "\"\<Units`\>\"", "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"robotInfo", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
       "lt", ",", "lh", ",", "La", ",", "Lf", ",", "Lc", ",", "Lt", ",", "LT",
         ",", "rfz", ",", "rfx", ",", "rc", ",", "rt", ",", "rT", ",", "mT", 
        ",", "mt", ",", "mc", ",", "mf", ",", "g", ",", "mboom", ",", "Lb"}], 
       "}"}], ",", "           ", 
      RowBox[{"{", 
       RowBox[{"152.4", ",", "50.8", ",", "50.8", ",", 
        RowBox[{"lt", "+", "lh"}], ",", "406.4", ",", "406.4", ",", "251.1", 
        ",", "6.87", ",", "114.51", ",", "219.68", ",", "219.68", ",", 
        "251.1", ",", "10418.76", ",", "3865.06", ",", "3865.06", ",", "1151",
         ",", "9810", ",", "0", ",", "0"}], "}"}]}], "}"}]}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"robotInfo", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "lt", ",", "lh", ",", "La", ",", "Lf", ",", "Lc", ",", "Lt", ",", 
         "LT", ",", "rfz", ",", "rfx", ",", "rc", ",", "rt", ",", "rT", ",", 
         "mT", ",", "mt", ",", "mc", ",", "mf", ",", "g", ",", "mboom", ",", 
         "Lb"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"142.5", ",", "59.37", ",", "102.62", ",", 
         RowBox[{"lt", "+", "lh"}], ",", "343.13", ",", "298.45", ",", "63.5",
          ",", "35.6", ",", "24.73", ",", "176.13", ",", "163.95", ",", 
         "130.76", ",", "1820.99", ",", "1031.09", ",", "992.95", ",", 
         "233.03", ",", "9810", ",", "0", ",", "0"}], "}"}]}], "}"}]}], ";"}],
    "*)"}]}]}], "Input",
 CellChangeTimes->{{3.5430098460431986`*^9, 3.543010029064667*^9}, {
   3.543010081054641*^9, 3.5430100828627443`*^9}, {3.5430104661596675`*^9, 
   3.543010467149724*^9}, {3.5430105282602196`*^9, 3.543010528843253*^9}, 
   3.543319459240864*^9, 3.5433199723372116`*^9, 3.543861833886557*^9, {
   3.5452665214324265`*^9, 3.545266531262983*^9}, {3.5472211837958374`*^9, 
   3.5472213065868607`*^9}, {3.547223310501478*^9, 3.547223326932418*^9}, {
   3.547223366932706*^9, 3.5472233743961325`*^9}, {3.547225227568128*^9, 
   3.5472252341515045`*^9}, {3.5472280446202545`*^9, 
   3.5472280452362895`*^9}, {3.5472307883841887`*^9, 3.547230810752468*^9}, {
   3.5472310302560225`*^9, 3.5472310320151234`*^9}, {3.547231455301334*^9, 
   3.547231531428688*^9}, {3.547231898627691*^9, 3.547231901396849*^9}, {
   3.547232065874257*^9, 3.5472320678973722`*^9}, {3.5472418829337068`*^9, 
   3.547241895133384*^9}, {3.5473063624036665`*^9, 3.5473063806868987`*^9}, {
   3.5473064358017955`*^9, 3.547306500292309*^9}, {3.547306763110097*^9, 
   3.5473067796305265`*^9}, {3.547307408015522*^9, 3.5473074094536037`*^9}, {
   3.5473074466307306`*^9, 3.547307461930606*^9}, {3.547307538078905*^9, 
   3.5473076265860605`*^9}, {3.547307760449505*^9, 3.547307762871771*^9}, {
   3.547307979358945*^9, 3.547307981766074*^9}, {3.547308013191676*^9, 
   3.5473080149137735`*^9}, {3.547308470862816*^9, 3.5473084738627815`*^9}, {
   3.547767981008011*^9, 3.547767991564615*^9}, {3.5477680473948083`*^9, 
   3.5477680503309765`*^9}, {3.5477681357878637`*^9, 
   3.5477681422992363`*^9}, {3.5477682948959646`*^9, 
   3.5477683418896523`*^9}, {3.547830395030449*^9, 3.5478304025728807`*^9}, {
   3.5478995332029266`*^9, 3.5478995548271637`*^9}, {3.5515403169704046`*^9, 
   3.5515403927865376`*^9}, {3.5515405790820646`*^9, 3.5515405965384955`*^9}, 
   3.5515406387521696`*^9, {3.551540675942635*^9, 3.5515406783762393`*^9}, {
   3.5515407877396317`*^9, 3.551540834758114*^9}, {3.5528635458033614`*^9, 
   3.552863546098378*^9}, {3.552863639842456*^9, 3.5528636629137216`*^9}, {
   3.5528644781269016`*^9, 3.552864493187742*^9}, {3.5529318985790834`*^9, 
   3.552931904052397*^9}, {3.5529319431236315`*^9, 3.5529319557303524`*^9}, {
   3.5563272388691444`*^9, 3.5563272409032607`*^9}, 3.5563854214675245`*^9, {
   3.5813368368893337`*^9, 3.5813368393864765`*^9}, {3.581336894288617*^9, 
   3.5813369102235284`*^9}, {3.5813369554081125`*^9, 
   3.5813369564881744`*^9}, {3.581337036762271*^9, 3.5813370394984274`*^9}, {
   3.5813370702501864`*^9, 3.581337072026288*^9}, {3.5813372110662403`*^9, 
   3.5813372210828133`*^9}, {3.5813372787411118`*^9, 3.581337323050646*^9}, {
   3.5813373561385384`*^9, 3.5813373579546423`*^9}, {3.5926077491638546`*^9, 
   3.5926077516114864`*^9}, {3.592609465470059*^9, 3.592609523102479*^9}, {
   3.5926097928363028`*^9, 3.5926098317392373`*^9}, {3.5926101162659225`*^9, 
   3.592610118697543*^9}, {3.5926104027117214`*^9, 3.5926104031350036`*^9}, 
   3.5926104466440115`*^9, {3.593979363906705*^9, 3.5939793786175313`*^9}, {
   3.5939794282724185`*^9, 3.593979496007737*^9}, {3.593979573914274*^9, 
   3.5939795945375104`*^9}, {3.606238406732359*^9, 3.6062384128977118`*^9}, {
   3.606238448761763*^9, 3.6062384711030407`*^9}, {3.606238517219679*^9, 
   3.6062385949711256`*^9}, {3.6181523450971165`*^9, 
   3.6181523785750313`*^9}, {3.6181525710150385`*^9, 3.618152647567417*^9}}],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.5212045686835737`*^9, 3.521204606993765*^9}, {
   3.521204700067089*^9, 3.521204780986717*^9}, {3.5212145569110236`*^9, 
   3.5212145580280876`*^9}, {3.5212214210366507`*^9, 
   3.5212215159440784`*^9}, {3.521221549532*^9, 3.521221581995857*^9}, {
   3.5212225825050826`*^9, 3.5212225872403536`*^9}, {3.5212910875692406`*^9, 
   3.5212910900933847`*^9}, {3.5212911767343407`*^9, 
   3.5212911783804345`*^9}, {3.521291778077735*^9, 3.5212918161939154`*^9}, {
   3.521291985634607*^9, 3.521291991076918*^9}, {3.5212922081883364`*^9, 
   3.5212922159737816`*^9}, {3.5214849738620605`*^9, 3.521484978075301*^9}, {
   3.5214860083742313`*^9, 3.5214860164166913`*^9}, {3.522244545041182*^9, 
   3.522244546126244*^9}, {3.522244593734967*^9, 3.5222446610148153`*^9}, {
   3.522245182408637*^9, 3.5222451980385313`*^9}, {3.5222452480633926`*^9, 
   3.5222452884067*^9}, {3.5222453190144506`*^9, 3.522245320543538*^9}, {
   3.5222453895034823`*^9, 3.522245390398534*^9}, {3.5222454580864053`*^9, 
   3.5222454737583017`*^9}, {3.522508873288*^9, 3.522508878436*^9}, {
   3.52251692506606*^9, 3.5225169259961133`*^9}, {3.5226866514206*^9, 
   3.5226866515376*^9}, {3.5226870047536*^9, 3.5226870474105997`*^9}, {
   3.5226870857056*^9, 3.5226870879856*^9}, {3.5226871451626*^9, 
   3.5226871878106003`*^9}, 3.5230284498409986`*^9, 3.5418062039272957`*^9, {
   3.5418062378062334`*^9, 3.5418062543811817`*^9}, {3.5430100203221674`*^9, 
   3.5430100246494145`*^9}, 3.54776797486766*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"constsubs", "=", 
  RowBox[{
   RowBox[{"(", 
    RowBox[{
     RowBox[{
      RowBox[{"#1", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
      "\[Rule]", 
      FractionBox[
       RowBox[{"Rationalize", "[", 
        RowBox[{"#1", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
        "]"}], "1000"]}], "&"}], ")"}], "/@", 
   RowBox[{"Transpose", "[", "robotInfo", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.5197506180672736`*^9, 3.519750623492584*^9}, {
   3.5197506874442415`*^9, 3.519750758152286*^9}, {3.51975089075187*^9, 
   3.519750913356163*^9}, 3.521204776350452*^9, {3.5212215282457824`*^9, 
   3.521221530862932*^9}, {3.522245362849958*^9, 3.522245370044369*^9}, 
   3.5230284621307015`*^9, 3.523304585666345*^9, 3.523305467209157*^9, {
   3.523305668402233*^9, 3.523305669132444*^9}, {3.541806217485071*^9, 
   3.5418062335419893`*^9}, 3.5418579476383076`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"lt", "\[Rule]", 
    FractionBox["381", "2500"]}], ",", 
   RowBox[{"lh", "\[Rule]", 
    FractionBox["127", "2500"]}], ",", 
   RowBox[{"La", "\[Rule]", 
    FractionBox["127", "2500"]}], ",", 
   RowBox[{"Lf", "\[Rule]", 
    FractionBox[
     RowBox[{"lh", "+", "lt"}], "1000"]}], ",", 
   RowBox[{"Lc", "\[Rule]", 
    FractionBox["254", "625"]}], ",", 
   RowBox[{"Lt", "\[Rule]", 
    FractionBox["254", "625"]}], ",", 
   RowBox[{"LT", "\[Rule]", 
    FractionBox["2511", "10000"]}], ",", 
   RowBox[{"rfz", "\[Rule]", 
    FractionBox["687", "100000"]}], ",", 
   RowBox[{"rfx", "\[Rule]", 
    FractionBox["11451", "100000"]}], ",", 
   RowBox[{"rc", "\[Rule]", 
    FractionBox["1373", "6250"]}], ",", 
   RowBox[{"rt", "\[Rule]", 
    FractionBox["1373", "6250"]}], ",", 
   RowBox[{"rT", "\[Rule]", 
    FractionBox["2511", "10000"]}], ",", 
   RowBox[{"mT", "\[Rule]", 
    FractionBox["260469", "25000"]}], ",", 
   RowBox[{"mt", "\[Rule]", 
    FractionBox["193253", "50000"]}], ",", 
   RowBox[{"mc", "\[Rule]", 
    FractionBox["193253", "50000"]}], ",", 
   RowBox[{"mf", "\[Rule]", 
    FractionBox["1151", "1000"]}], ",", 
   RowBox[{"g", "\[Rule]", 
    FractionBox["981", "100"]}], ",", 
   RowBox[{"mboom", "\[Rule]", "0"}], ",", 
   RowBox[{"Lb", "\[Rule]", "0"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.541857948621109*^9, 3.5418579585427265`*^9}, 
   3.541863037898039*^9, 3.541868242954465*^9, 3.541870792073408*^9, 
   3.541871715289483*^9, 3.542471356657548*^9, 3.5424724470647354`*^9, 
   3.542479959915756*^9, {3.542484016345809*^9, 3.5424840355650425`*^9}, 
   3.5424944091496534`*^9, 3.5425426437119193`*^9, 3.542542983414349*^9, 
   3.54254860777427*^9, 3.5425566254552536`*^9, 3.542557138912012*^9, 
   3.54256839918017*^9, 3.5425692505683146`*^9, 3.5427289212609444`*^9, 
   3.542730110315833*^9, 3.5427317413103*^9, 3.5427336899604545`*^9, 
   3.5427344088858776`*^9, 3.5427344564901314`*^9, 3.5427345838741016`*^9, 
   3.5427347262805767`*^9, 3.542734860569558*^9, 3.5427351697481127`*^9, 
   3.5427352531067386`*^9, 3.5427353550897627`*^9, 3.5427361428557177`*^9, 
   3.542736194609562*^9, 3.5427367829862785`*^9, 3.5427368557864227`*^9, 
   3.542900086311864*^9, 3.5429021365358677`*^9, 3.5429046256146393`*^9, 
   3.5429070810767107`*^9, 3.542931185014475*^9, 3.5429313413034143`*^9, 
   3.5429844138158064`*^9, 3.542985283372542*^9, 3.5429875020074406`*^9, 
   3.5429980707099366`*^9, 3.5429999978749924`*^9, 3.543010030836768*^9, 
   3.5430100862579384`*^9, 3.5430103122358637`*^9, 3.5430103507480664`*^9, {
   3.5430105211308117`*^9, 3.5430105323524537`*^9}, 3.543010675892664*^9, 
   3.5430108747140355`*^9, 3.543079380075317*^9, 3.5430796458932514`*^9, 
   3.543080305333012*^9, 3.543104204121504*^9, 3.5431061886539035`*^9, 
   3.543257637658081*^9, 3.543319464123143*^9, 3.543319976350441*^9, 
   3.543363847178707*^9, 3.5438618369177303`*^9, 3.5447124473153315`*^9, 
   3.545265830238581*^9, 3.5452666382385445`*^9, 3.545769013718904*^9, 
   3.5472213121401787`*^9, 3.5472280000827065`*^9, 3.547228048294464*^9, 
   3.5472283963743734`*^9, 3.5472292910455456`*^9, 3.5472296779236736`*^9, 
   3.5472297521039166`*^9, 3.547229964111043*^9, {3.547230814893705*^9, 
   3.5472308315866594`*^9}, 3.5472317831770873`*^9, 3.547231912933509*^9, 
   3.5472319548659077`*^9, 3.54723207030151*^9, 3.547240695099575*^9, 
   3.5472407969035892`*^9, 3.5472408352833347`*^9, 3.547241934187392*^9, 
   3.5473093524727974`*^9, 3.547402744133217*^9, 3.5474032451018705`*^9, 
   3.5474035224727354`*^9, 3.5474915094023*^9, 3.5477685168136573`*^9, 
   3.5478304104193296`*^9, 3.547899564820735*^9, 3.547899814699027*^9, 
   3.5515414190103397`*^9, 3.552650596948243*^9, {3.5526529010410295`*^9, 
   3.552652920327132*^9}, 3.5528284618368406`*^9, 3.5529323647377462`*^9, 
   3.5529325123461895`*^9, 3.5563177933421755`*^9, 3.5563253338372774`*^9, 
   3.5563273112562847`*^9, 3.5563854519212666`*^9, 3.556833533859611*^9, 
   3.581337405786378*^9, 3.588020768901226*^9, 3.592661504500222*^9, 
   3.6192631140094166`*^9, 3.6192632458784904`*^9, 3.619263326053218*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"ndof", " ", "=", " ", "7"}], ";"}]], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.5158694971113553`*^9, 3.51586950751295*^9}, {
   3.515885996605161*^9, 3.5158859966675615`*^9}, 3.5418062596754847`*^9, 
   3.5418580462616806`*^9, {3.542730064753753*^9, 3.5427300744257703`*^9}}],

Cell[BoxData[{
 RowBox[{"mm", "=", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
    "mf", ",", " ", "mc", ",", "mt", ",", " ", "mT", ",", "0", ",", "mt", ",",
      "mc", ",", " ", "mf"}], "}"}], "/.", "\[InvisibleSpace]", 
   "constsubs"}]}], "\n", 
 RowBox[{"statesubs", "=", 
  RowBox[{"Join", "[", 
   RowBox[{
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
       RowBox[{"x", "[", "i", "]"}]}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "1", ",", 
        RowBox[{"ndof", "-", "1"}]}], "}"}]}], "]"}], ",", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "i"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
       RowBox[{"x", "[", 
        RowBox[{"i", "+", "ndof", "-", "1"}], "]"}]}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "1", ",", 
        RowBox[{"ndof", "-", "1"}]}], "}"}]}], "]"}]}], "]"}]}]}], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.504464931380743*^9, 3.504464938859278*^9}, {
   3.50446501108097*^9, 3.504465011431731*^9}, {3.504465051759923*^9, 
   3.504465055474*^9}, {3.504617746752343*^9, 3.5046177488414*^9}, 
   3.504747296129977*^9, 3.50487586421991*^9, {3.504971551457563*^9, 
   3.50497170345489*^9}, {3.504971751954021*^9, 3.504971771812788*^9}, {
   3.5052271115942125`*^9, 3.5052271200806274`*^9}, {3.505231180364644*^9, 
   3.505231192454666*^9}, {3.50523314771557*^9, 3.5052331520835776`*^9}, {
   3.513011680554058*^9, 3.513011725186837*^9}, 3.51301182562513*^9, {
   3.513012344045216*^9, 3.513012345041097*^9}, {3.5158498204093294`*^9, 
   3.515849822884471*^9}, {3.515850217828061*^9, 3.5158502194581537`*^9}, {
   3.515850546875881*^9, 3.5158505634158273`*^9}, {3.515850617926945*^9, 
   3.515850624852341*^9}, {3.5158506825046387`*^9, 3.515850684811771*^9}, {
   3.5158510414861712`*^9, 3.5158510548999386`*^9}, {3.5158522964848585`*^9, 
   3.5158522987139854`*^9}, {3.5158524456553907`*^9, 
   3.5158524490995874`*^9}, {3.5158693992257566`*^9, 3.515869425043233*^9}, {
   3.515869486273735*^9, 3.515869512859256*^9}, {3.516707936034129*^9, 
   3.516707940618391*^9}, 3.516708850637441*^9, {3.5214849611743345`*^9, 
   3.521484964012497*^9}, {3.522244667184168*^9, 3.5222446699923286`*^9}, {
   3.5418062795246196`*^9, 3.5418062894351864`*^9}, 3.5418063201219416`*^9, {
   3.5418579738151536`*^9, 3.5418579866695757`*^9}, 3.5418580462616806`*^9, {
   3.5427339159796762`*^9, 3.542733926746468*^9}, {3.5427339636111226`*^9, 
   3.542733972907631*^9}, {3.5427340540706244`*^9, 3.5427340754194627`*^9}, {
   3.5430100938623734`*^9, 3.5430101032219086`*^9}, 3.543104199445236*^9, 
   3.5432576399242105`*^9, {3.5472242259738398`*^9, 3.5472242262458553`*^9}, 
   3.5474913975659037`*^9, {3.5474914886421127`*^9, 3.5474914903172083`*^9}}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   FractionBox["1151", "1000"], ",", 
   FractionBox["193253", "50000"], ",", 
   FractionBox["193253", "50000"], ",", 
   FractionBox["260469", "25000"], ",", "0", ",", 
   FractionBox["193253", "50000"], ",", 
   FractionBox["193253", "50000"], ",", 
   FractionBox["1151", "1000"]}], "}"}]], "Output",
 CellChangeTimes->{
  3.543257640285231*^9, 3.5433194641401443`*^9, 3.543319976369442*^9, 
   3.543363847197708*^9, 3.543861836964733*^9, 3.5447124473443327`*^9, 
   3.545265830265583*^9, 3.5452666382735443`*^9, 3.5457690137619066`*^9, 
   3.5472213121791806`*^9, 3.5472280001477103`*^9, 3.547228048341467*^9, 
   3.5472283964143753`*^9, 3.547229291086548*^9, 3.547229677949675*^9, 
   3.547229752141919*^9, 3.5472299641740465`*^9, {3.547230814937707*^9, 
   3.547230831626662*^9}, 3.547231783202089*^9, 3.547231912971511*^9, 
   3.5472319549109097`*^9, 3.547232070333512*^9, 3.5472406952445755`*^9, 
   3.547240797122602*^9, 3.547240835396335*^9, 3.5472419342973986`*^9, 
   3.5473093525007987`*^9, 3.54740274417822*^9, 3.547403245159874*^9, 
   3.547403522521738*^9, 3.5474913979639263`*^9, {3.54749150572009*^9, 
   3.5474915094403024`*^9}, 3.5477685168746605`*^9, 3.5478304104543314`*^9, 
   3.5478995648547373`*^9, 3.5478998147290287`*^9, 3.5515414190727406`*^9, 
   3.5526505969752445`*^9, {3.5526529010680313`*^9, 3.552652920360134*^9}, 
   3.552828461861842*^9, 3.552932364762748*^9, 3.5529325123631897`*^9, 
   3.556317793393179*^9, 3.556325333907282*^9, 3.556327311301287*^9, 
   3.55638545198227*^9, 3.556833533942226*^9, 3.5813374058163795`*^9, 
   3.588020768927228*^9, 3.5926615045242233`*^9, 3.619263114025426*^9, 
   3.619263245897502*^9, 3.6192633260672207`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "1", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "2", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "3", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "4", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "5", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "6"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "6", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "1"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "7", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "2"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "8", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "3"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "9", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "4"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "10", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "5"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "11", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "6"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "12", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.543257640285231*^9, 3.5433194641401443`*^9, 3.543319976369442*^9, 
   3.543363847197708*^9, 3.543861836964733*^9, 3.5447124473443327`*^9, 
   3.545265830265583*^9, 3.5452666382735443`*^9, 3.5457690137619066`*^9, 
   3.5472213121791806`*^9, 3.5472280001477103`*^9, 3.547228048341467*^9, 
   3.5472283964143753`*^9, 3.547229291086548*^9, 3.547229677949675*^9, 
   3.547229752141919*^9, 3.5472299641740465`*^9, {3.547230814937707*^9, 
   3.547230831626662*^9}, 3.547231783202089*^9, 3.547231912971511*^9, 
   3.5472319549109097`*^9, 3.547232070333512*^9, 3.5472406952445755`*^9, 
   3.547240797122602*^9, 3.547240835396335*^9, 3.5472419342973986`*^9, 
   3.5473093525007987`*^9, 3.54740274417822*^9, 3.547403245159874*^9, 
   3.547403522521738*^9, 3.5474913979639263`*^9, {3.54749150572009*^9, 
   3.5474915094403024`*^9}, 3.5477685168746605`*^9, 3.5478304104543314`*^9, 
   3.5478995648547373`*^9, 3.5478998147290287`*^9, 3.5515414190727406`*^9, 
   3.5526505969752445`*^9, {3.5526529010680313`*^9, 3.552652920360134*^9}, 
   3.552828461861842*^9, 3.552932364762748*^9, 3.5529325123631897`*^9, 
   3.556317793393179*^9, 3.556325333907282*^9, 3.556327311301287*^9, 
   3.55638545198227*^9, 3.556833533942226*^9, 3.5813374058163795`*^9, 
   3.588020768927228*^9, 3.5926615045242233`*^9, 3.619263114025426*^9, 
   3.619263245897502*^9, 3.6192633260712214`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"exstatesubs", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", "1", "]"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", "2", "]"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", "3", "]"}]}]}], "}"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", 
         RowBox[{"i", "+", "3"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", 
         RowBox[{"ndof", "-", "1"}]}], "}"}]}], "]"}], ",", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["p", "x"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", "10", "]"}]}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["p", "z"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", "11", "]"}]}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "0"], "\[Prime]"], "[", "t", "]"}], 
        "\[Rule]", 
        RowBox[{"x", "[", "12", "]"}]}]}], "}"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["\[Theta]", "i"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"x", "[", 
         RowBox[{"i", "+", "ndof", "+", "5"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", 
         RowBox[{"ndof", "-", "1"}]}], "}"}]}], "]"}]}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5427339861542597`*^9, 3.5427340471636505`*^9}, {
   3.54273413433239*^9, 3.5427342330213256`*^9}, {3.542734283709563*^9, 
   3.5427343130212274`*^9}, {3.5429313280416555`*^9, 3.542931337464195*^9}, 
   3.5431042083882647`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[ScriptCapitalI]", "i"], "=", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", "3", "}"}], ",", 
         RowBox[{"{", "3", "}"}]}], "]"}]}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ";"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "1"], " ", "=", " ", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"(", GridBox[{
           {"1541037.64", "53192.75", 
            RowBox[{"-", "722846.62"}]},
           {"53192.75", "8982671.3", 
            RowBox[{"-", "22618.87"}]},
           {
            RowBox[{"-", "722846.62"}], 
            RowBox[{"-", "22618.87"}], "8498063.5"}
          }], ")"}], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "2"], " ", "=", "  ", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"(", GridBox[{
           {"29032438.82", 
            RowBox[{"-", "153737.5"}], "191685.6"},
           {
            RowBox[{"-", "153737.5"}], "28425223.81", "472879.37"},
           {"191685.6", "472879.37", "3199555.09"}
          }], ")"}], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "3"], " ", "=", " ", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"(", GridBox[{
           {"29032438.82", 
            RowBox[{"-", "153737.5"}], "191685.6"},
           {
            RowBox[{"-", "153737.5"}], "28425223.81", "472879.37"},
           {"191685.6", "472879.37", "3199555.09"}
          }], ")"}], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "4"], " ", "=", "  ", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"(", GridBox[{
           {"188576345.34", 
            RowBox[{"-", "40410.33"}], 
            RowBox[{"-", "2438220.72"}]},
           {
            RowBox[{"-", "40410.33"}], "147542602", 
            RowBox[{"-", "414660.3"}]},
           {
            RowBox[{"-", "2438220.72"}], 
            RowBox[{"-", "414660.3"}], "59483657.62"}
          }], ")"}], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"(", GridBox[{
           {"0", "0", "0"},
           {"0", "0", "0"},
           {"0", "0", "0"}
          }], ")"}], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "6"], " ", "=", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalI]", "3"], "*", " ", 
      RowBox[{"(", GridBox[{
         {"1", "1", 
          RowBox[{"-", "1"}]},
         {"1", "1", 
          RowBox[{"-", "1"}]},
         {
          RowBox[{"-", "1"}], 
          RowBox[{"-", "1"}], "1"}
        }], ")"}]}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "7"], " ", "=", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalI]", "2"], "*", " ", 
      RowBox[{"(", GridBox[{
         {"1", "1", 
          RowBox[{"-", "1"}]},
         {"1", "1", 
          RowBox[{"-", "1"}]},
         {
          RowBox[{"-", "1"}], 
          RowBox[{"-", "1"}], "1"}
        }], ")"}]}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "8"], " ", "=", 
     SubscriptBox["\[ScriptCapitalI]", "1"]}], ";"}], 
   "\[IndentingNewLine]"}]}]], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{
  3.5418064236368628`*^9, 3.5418580462616806`*^9, {3.5418581242742195`*^9, 
   3.5418581281586266`*^9}, 3.5427301366330795`*^9, {3.547223017917743*^9, 
   3.547223058841084*^9}, {3.5472242758596935`*^9, 3.5472242889504423`*^9}, {
   3.547308080188949*^9, 3.5473080831001115`*^9}, {3.5473085270683513`*^9, 
   3.547308620152511*^9}, 3.5473086590924053`*^9, {3.5477681671776595`*^9, 
   3.5477682722366686`*^9}, {3.5477683742145014`*^9, 
   3.5477684676058426`*^9}, {3.551540975329961*^9, 3.551541026529251*^9}, {
   3.551541058556107*^9, 3.551541093874569*^9}, {3.551541136493844*^9, 
   3.5515411617346883`*^9}, {3.551541235507218*^9, 3.551541286893708*^9}, {
   3.5515413487790165`*^9, 3.5515413911954913`*^9}, {3.5528638987663126`*^9, 
   3.552863983761888*^9}, {3.5528640371237135`*^9, 3.5528641201691628`*^9}, {
   3.552864171088937*^9, 3.5528642363844185`*^9}, {3.552864309576182*^9, 
   3.552864415650615*^9}, {3.552931994343561*^9, 3.5529320036230917`*^9}, {
   3.552932094973317*^9, 3.552932131815424*^9}, {3.55293220167942*^9, 
   3.5529322654550676`*^9}, {3.5529322968318624`*^9, 3.5529323495678787`*^9}, 
   3.556327250596815*^9, 3.5563854399285803`*^9, {3.5939746238005157`*^9, 
   3.593974972478459*^9}, {3.606238608908923*^9, 3.6062390044915495`*^9}, {
   3.618152677292117*^9, 3.618152928962512*^9}}],

Cell["INCLUDING THE BOOM INERTIA", "Text",
 CellChangeTimes->{{3.547223667046871*^9, 3.5472236745833025`*^9}}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   SubscriptBox["I", "BOOM"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"8.85", ",", "0.07", ",", "0.15"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.07", ",", "0.82", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0.15", ",", "0", ",", "8.35"}], "}"}]}], "}"}], "/", 
       SuperscriptBox[
        RowBox[{"(", 
         RowBox[{"83.5", "*", ".0254"}], ")"}], "2"]}], ")"}], ",", 
     FractionBox["1", "1000000000"]}], "]"}]}], "*)"}]], "Input",
 CellChangeTimes->{{3.552652770147543*^9, 3.5526528467809258`*^9}, 
   3.556317786030758*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["I", "BOOM"], "=", 
  RowBox[{"Rationalize", "[", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{"15.83", ",", 
          RowBox[{"-", "0.11"}], ",", "0.06"}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"-", "0.11"}], ",", "5.67", ",", 
          RowBox[{"-", "0.29"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"0.06", ",", 
          RowBox[{"-", "0.29"}], ",", "10.39"}], "}"}]}], "}"}], "/", 
      SuperscriptBox[
       RowBox[{"(", 
        RowBox[{"83.5", "*", ".0254"}], ")"}], "2"]}], ")"}], ",", 
    FractionBox["1", "1000000000"]}], "]"}]}]], "Input",
 CellChangeTimes->{{3.55632526621541*^9, 3.5563253138591347`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     FractionBox["77552", "22037"], ",", 
     RowBox[{"-", 
      FractionBox["1503", "61462"]}], ",", 
     FractionBox["471", "35311"]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox["1503", "61462"]}], ",", 
     FractionBox["24731", "19620"], ",", 
     RowBox[{"-", 
      FractionBox["2389", "37056"]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox["471", "35311"], ",", 
     RowBox[{"-", 
      FractionBox["2389", "37056"]}], ",", 
     FractionBox["104156", "45093"]}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.5526527883625846`*^9, 3.55265281696422*^9}, {
   3.5526529011100335`*^9, 3.552652920407137*^9}, 3.552828461906844*^9, 
   3.55293236479675*^9, 3.5529325123911915`*^9, 3.5563177935251865`*^9, 
   3.556325334006287*^9, 3.556327311368291*^9, 3.5563854520852757`*^9, 
   3.5568335340734043`*^9, 3.5813374059173856`*^9, 3.588020769120239*^9, 
   3.592661504583227*^9, 3.6192631140804586`*^9, 3.619263245940528*^9, 
   3.619263326097226*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", " ", 
    RowBox[{"Length", "[", "mm", "]"}]}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["MM", "i"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
       " ", 
       RowBox[{"IdentityMatrix", "[", "3", "]"}]}], "+", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
         "0", ",", "0", ",", "0", ",", "0", ",", "1", ",", "0", ",", "0", ",",
           "0"}], "}"}], "\[LeftDoubleBracket]", "i", 
        "\[RightDoubleBracket]"}], 
       SubscriptBox["I", "BOOM"]}]}], "/.", "constsubs"}]}]}], 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", 
    RowBox[{"Length", "[", "mm", "]"}]}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        SubscriptBox["MM", "i"], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", 
        SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", "1"}],
      "]"}]}]}], "]"}]}], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{
  3.541858046277281*^9, {3.5472232844829903`*^9, 3.5472232856290555`*^9}, {
   3.5472233848837323`*^9, 3.547223392220152*^9}, {3.5472237228100605`*^9, 
   3.5472237234180956`*^9}, {3.5472239398394737`*^9, 
   3.5472239465458574`*^9}, {3.5472243528870993`*^9, 3.5472243537501483`*^9}, 
   3.5526506334983335`*^9, {3.55265265261082*^9, 3.552652748466303*^9}, 
   3.5526527845873685`*^9, {3.5526528288248987`*^9, 3.5526528586086025`*^9}, {
   3.5526528960237427`*^9, 3.5526528969347944`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 SubscriptBox["MM", "5"]], "Input",
 CellChangeTimes->{{3.552650690652602*^9, 3.552650710681748*^9}, {
  3.552650763097746*^9, 3.552650771632234*^9}, {3.5526527571948023`*^9, 
  3.552652757601825*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     FractionBox["77552", "22037"], ",", 
     RowBox[{"-", 
      FractionBox["1503", "61462"]}], ",", 
     FractionBox["471", "35311"]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox["1503", "61462"]}], ",", 
     FractionBox["24731", "19620"], ",", 
     RowBox[{"-", 
      FractionBox["2389", "37056"]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox["471", "35311"], ",", 
     RowBox[{"-", 
      FractionBox["2389", "37056"]}], ",", 
     FractionBox["104156", "45093"]}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.552650763928794*^9, 3.5526507720452576`*^9}, 
   3.5526527579008427`*^9, {3.552652901140035*^9, 3.552652920441139*^9}, 
   3.5528284619358463`*^9, 3.552932364822751*^9, 3.5529325124131927`*^9, 
   3.5563177936111913`*^9, 3.556325334076291*^9, 3.5563273114202943`*^9, 
   3.5563854521462793`*^9, 3.556833534144802*^9, 3.5813374059513874`*^9, 
   3.5880207691472406`*^9, 3.5926615046122284`*^9, 3.619263114095468*^9, 
   3.6192632459555364`*^9, 3.619263326111229*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 SubscriptBox["\[ScriptCapitalM]", "5"]], "Input",
 CellChangeTimes->{{3.5526507268306713`*^9, 3.552650736281212*^9}, {
  3.55265291013855*^9, 3.552652910705582*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     FractionBox["77552", "22037"], ",", 
     RowBox[{"-", 
      FractionBox["1503", "61462"]}], ",", 
     FractionBox["471", "35311"], ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
   
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox["1503", "61462"]}], ",", 
     FractionBox["24731", "19620"], ",", 
     RowBox[{"-", 
      FractionBox["2389", "37056"]}], ",", "0", ",", "0", ",", "0"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox["471", "35311"], ",", 
     RowBox[{"-", 
      FractionBox["2389", "37056"]}], ",", 
     FractionBox["104156", "45093"], ",", "0", ",", "0", ",", "0"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{{3.552650694049797*^9, 3.5526507365172253`*^9}, {
   3.5526529011630363`*^9, 3.5526529204641404`*^9}, 3.5528284619598475`*^9, 
   3.5529323648447523`*^9, 3.552932512423194*^9, 3.556317793682195*^9, 
   3.5563253341332946`*^9, 3.556327311461296*^9, 3.556385452194282*^9, 
   3.556833534194171*^9, 3.581337405980389*^9, 3.588020769169242*^9, 
   3.59266150463723*^9, 3.619263114104473*^9, 3.619263245964542*^9, 
   3.6192633261192307`*^9}]
}, Open  ]],

Cell[BoxData[""], "Input",
 CellChangeTimes->{{3.552650698390045*^9, 3.552650699985136*^9}}],

Cell["INCLUDING THE MOTOR INERTIA", "Text",
 CellChangeTimes->{{3.547223827774064*^9, 3.5472238325023346`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", " ", 
    RowBox[{"Length", "[", "mm", "]"}]}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["Mt", "i"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "0", ",", "1", ",", "1", ",", "1", ",", "0", ",", "1", ",", "1", ",", 
         "1"}], "}"}], "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}],
       " ", 
      RowBox[{"Rationalize", "[", 
       RowBox[{
        FractionBox[
         RowBox[{"MKS", "[", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{"21.9", " ", 
              SuperscriptBox["95.04", "2"]}], "+", "1.5"}], ")"}], " ", 
           "Gram", " ", 
           SuperscriptBox["Centimeter", "2"]}], "]"}], 
         RowBox[{"Kilogram", " ", 
          SuperscriptBox["Meter", "2"]}]], ",", 
        FractionBox["1", "100000000"]}], "]"}]}], "/.", "constsubs"}]}]}], 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"For", "[", 
    RowBox[{
     RowBox[{"i", "=", "1"}], ",", 
     RowBox[{"i", "\[LessEqual]", 
      RowBox[{"Length", "[", "mm", "]"}]}], ",", 
     RowBox[{"i", "++"}], ",", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Join", "[", 
         RowBox[{
          SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
          RowBox[{"Table", "[", 
           RowBox[{"0", ",", 
            RowBox[{"{", "3", "}"}], ",", 
            RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
        RowBox[{"Join", "[", 
         RowBox[{
          RowBox[{"Table", "[", 
           RowBox[{"0", ",", 
            RowBox[{"{", "3", "}"}], ",", 
            RowBox[{"{", "3", "}"}]}], "]"}], ",", 
          SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", 
        "1"}], "]"}]}]}], "]"}], "*)"}]}]}], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.5214873029990063`*^9, 3.521487319252936*^9}, 
   3.521487394350231*^9, {3.5214875890623684`*^9, 3.521487593325612*^9}, {
   3.5230262518716383`*^9, 3.5230262525626774`*^9}, {3.523026311396043*^9, 
   3.523026330981163*^9}, 3.5230274909066725`*^9, 3.5230275221313057`*^9, {
   3.523030604303975*^9, 3.523030607054125*^9}, {3.523811329135682*^9, 
   3.523811338183199*^9}, 3.5238113734532166`*^9, 3.5238114749630227`*^9, 
   3.541858046277281*^9, 3.5472238432149477`*^9, {3.5472239668260174`*^9, 
   3.547223983536974*^9}, {3.5472244498156433`*^9, 3.5472244522357817`*^9}, {
   3.5472244997024965`*^9, 3.547224533614436*^9}, {3.547224675574556*^9, 
   3.547224676212592*^9}, {3.5472247754282675`*^9, 3.5472247770593605`*^9}, {
   3.55154090665864*^9, 3.551540907610242*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"p0", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "0"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     
     RowBox[{
      RowBox[{
       SuperscriptBox[
        SubscriptBox["p", "x"], "\[Prime]",
        MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SuperscriptBox[
        SubscriptBox["p", "z"], "\[Prime]",
        MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SuperscriptBox[
        SubscriptBox["\[Theta]", "0"], "\[Prime]"], "[", "t", "]"}], 
      "\[Rule]", "0"}]}], "}"}]}], ";"}]], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.513532355258184*^9, 3.513532394514388*^9}, {
   3.5158587679930077`*^9, 3.515858770152131*^9}, {3.5167081862334394`*^9, 
   3.516708188824588*^9}, {3.521292324323979*^9, 3.5212923363016644`*^9}, {
   3.522411783001712*^9, 3.5224117989146223`*^9}, {3.5224118477114134`*^9, 
   3.522411851376623*^9}, 3.541858046277281*^9, {3.542730087794994*^9, 
   3.5427300971238103`*^9}, {3.542734088616539*^9, 3.5427341229389677`*^9}, {
   3.542734840921886*^9, 3.542734857753604*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["generalized coordinates", "Subsection",
 CellChangeTimes->{
  3.504391119483818*^9, {3.513012126441401*^9, 3.51301212962473*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"q", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", 
       RowBox[{"ndof", "-", "1"}]}], "}"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"dq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ddq", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391169851249*^9, 3.504391191970865*^9}, {
   3.5129535326917257`*^9, 3.5129535355376797`*^9}, 3.513011937181409*^9, {
   3.513012138013254*^9, 3.513012139016991*^9}, 3.515858456069166*^9, {
   3.5158695182095623`*^9, 3.515869519316625*^9}, {3.541806510608837*^9, 
   3.5418065120889215`*^9}, {3.5418128180025992`*^9, 
   3.5418128209387674`*^9}, {3.541812901623382*^9, 3.5418129418366823`*^9}, {
   3.5418580518152905`*^9, 3.541858054436095*^9}, {3.542730149768303*^9, 
   3.542730150563904*^9}, {3.5427343384327965`*^9, 3.542734340290454*^9}, {
   3.5427348250964565`*^9, 3.5427348273853817`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"qe", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}], "}"}]}], "}"}], ",", 
     "q", ",", "1"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"dqe", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513012131467762*^9, 3.513012222132253*^9}, 
   3.51353184821528*^9, 3.5158584591123405`*^9, {3.515859715694213*^9, 
   3.5158597176283236`*^9}, {3.5418064988411636`*^9, 3.541806507218643*^9}, {
   3.541858033329258*^9, 3.5418580626729097`*^9}, {3.5425486273283887`*^9, 
   3.5425486298405323`*^9}, {3.542730158457518*^9, 3.542730184774764*^9}, 
   3.542734830616359*^9, 3.5429000795414524`*^9}],

Cell[CellGroupData[{

Cell["location and direction of twists", "Subsubsection",
 CellChangeTimes->{
  3.504391222051317*^9, {3.513012004436311*^9, 3.513012017657574*^9}}],

Cell["positive z direction", "Text",
 CellChangeTimes->{{3.504895806007092*^9, 3.504895813878101*^9}, {
  3.504899673358274*^9, 3.504899680392001*^9}, {3.5427302505912795`*^9, 
  3.542730251792482*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "0"], "=", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "px"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "pz"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"PrismaticTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  SubscriptBox["\[Xi]", "q0"], "=", 
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{"RevoluteTwist", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"0", ",", 
        RowBox[{"-", "1"}], ",", "0"}], "}"}], "/.", "constsubs"}]}], "]"}], 
   "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q1"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "La"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q2"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "Lc"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q3"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", 
         RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q4"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "Lc", "+", "Lt"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q5"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "Lc"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Xi]", "q6"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RevoluteTwist", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "La"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504391239055382*^9, 3.504391280546184*^9}, {
   3.504393690323417*^9, 3.504393694642943*^9}, {3.504394354402188*^9, 
   3.504394382731842*^9}, 3.504394560552597*^9, {3.504733962343795*^9, 
   3.50473399215855*^9}, {3.504734047207357*^9, 3.504734112723113*^9}, {
   3.504895790317638*^9, 3.504895793671604*^9}, {3.504895835178937*^9, 
   3.504895837468827*^9}, {3.50489969948502*^9, 3.504899711184298*^9}, {
   3.504901355868927*^9, 3.504901367126576*^9}, {3.504901644110264*^9, 
   3.504901651461383*^9}, {3.5125072468769054`*^9, 3.5125072492793093`*^9}, {
   3.512510523268261*^9, 3.512510626680843*^9}, {3.512510703620178*^9, 
   3.5125107228706117`*^9}, {3.5125107694990935`*^9, 
   3.5125107706690955`*^9}, {3.512510874416078*^9, 3.5125108760228806`*^9}, {
   3.5125109282169724`*^9, 3.5125109524594145`*^9}, 3.512511205857059*^9, {
   3.5125116217689886`*^9, 3.5125116245925937`*^9}, {3.512512838505127*^9, 
   3.5125128440899363`*^9}, 3.5125154382821627`*^9, 3.512515631401702*^9, {
   3.512515675315779*^9, 3.512515677234583*^9}, {3.512515713489046*^9, 
   3.51251571545465*^9}, {3.5125157856379766`*^9, 3.5125157895691843`*^9}, {
   3.5125160948139267`*^9, 3.512516101631139*^9}, {3.5125163059110966`*^9, 
   3.512516337906753*^9}, 3.5125169799917154`*^9, 3.512517350494175*^9, {
   3.5125175771075783`*^9, 3.512517615811246*^9}, {3.5125182182931213`*^9, 
   3.5125182275127373`*^9}, {3.513011948790205*^9, 3.513011986836761*^9}, {
   3.513012028383106*^9, 3.513012030110642*^9}, {3.513531858718806*^9, 
   3.513531893067239*^9}, {3.513869388669607*^9, 3.513869391129299*^9}, {
   3.513956274948754*^9, 3.513956281701611*^9}, {3.5155018106921597`*^9, 
   3.515501813883342*^9}, {3.515501850954462*^9, 3.5155018542846527`*^9}, {
   3.5158584657537203`*^9, 3.515858513238436*^9}, {3.515869263882015*^9, 
   3.5158692653601*^9}, {3.5158703231956043`*^9, 3.515870326599799*^9}, {
   3.515886006277178*^9, 3.5158860120335884`*^9}, {3.5167081937628703`*^9, 
   3.5167082227325273`*^9}, 3.51670884415207*^9, {3.5418074444672503`*^9, 
   3.541807512144121*^9}, {3.541807551558376*^9, 3.541807580197014*^9}, {
   3.5418081022288723`*^9, 3.5418081246781564`*^9}, {3.5418092778921165`*^9, 
   3.5418093036105876`*^9}, {3.5418105140608215`*^9, 
   3.5418105241954007`*^9}, {3.5418114094460344`*^9, 3.541811435316514*^9}, {
   3.541812948721076*^9, 3.541812956384514*^9}, 3.541858299743328*^9, {
   3.5418592927854695`*^9, 3.541859326543929*^9}, {3.5425426329153013`*^9, 
   3.542542641067768*^9}, {3.5425429417109632`*^9, 3.5425429534616356`*^9}, {
   3.5425682062398987`*^9, 3.542568210870906*^9}, {3.5427292930563974`*^9, 
   3.5427293451916895`*^9}, {3.5427350714891305`*^9, 3.5427351619514737`*^9}, 
   3.5427361888172445`*^9, 3.5429844207022*^9, 3.542985277885228*^9, {
   3.5430101410880747`*^9, 3.543010171630821*^9}, {3.5447123218211536`*^9, 
   3.5447123296546016`*^9}, {3.545266552065091*^9, 3.545266554821249*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", 
   RowBox[{"-", "1"}], ",", "0"}], "}"}]], "Output",
 CellChangeTimes->{
  3.542984462347582*^9, 3.542985283458547*^9, 3.542987502093446*^9, 
   3.542998072026012*^9, 3.5429999979685926`*^9, 3.543010313686947*^9, 
   3.543010352187149*^9, 3.5430106774297514`*^9, 3.5430108748380423`*^9, 
   3.543079381633975*^9, 3.543079646018052*^9, 3.5430803054578123`*^9, 
   3.54310420545758*^9, 3.543104287984192*^9, 3.543106249001355*^9, 
   3.543319464182147*^9, 3.5433199764064445`*^9, 3.543363848466781*^9, 
   3.5438618385368233`*^9, 3.544712448707411*^9, 3.5452658315066524`*^9, 
   3.5452666383385444`*^9, 3.5457690150289793`*^9, 3.5472213134852552`*^9, 
   3.547228002131824*^9, 3.5472280484524736`*^9, 3.54722839649438*^9, 
   3.5472292923856225`*^9, 3.54722967926375*^9, 3.547229752207923*^9, 
   3.547229964294053*^9, {3.5472308150167117`*^9, 3.5472308317096663`*^9}, 
   3.5472317832680926`*^9, 3.5472319130365148`*^9, 3.5472319562279854`*^9, 
   3.5472320703985157`*^9, 3.5472406956195755`*^9, 3.5472407974416203`*^9, 
   3.5472408393224983`*^9, 3.5472419345554132`*^9, 3.5473093537378693`*^9, 
   3.547402745342286*^9, 3.5474032452698803`*^9, 3.547403522600743*^9, 
   3.5474915105643663`*^9, 3.5477685185777583`*^9, 3.5478304115903964`*^9, 
   3.5478995659788013`*^9, 3.5478998159981017`*^9, 3.551541420289542*^9, 
   3.552650598086308*^9, {3.552652901205039*^9, 3.5526529205081425`*^9}, 
   3.5528284631469154`*^9, 3.5529323659598165`*^9, 3.552932513518256*^9, 
   3.5563177951572795`*^9, 3.556325335203356*^9, 3.5563273115493016`*^9, 
   3.5563854534173517`*^9, 3.556833535590282*^9, 3.5813374060713944`*^9, 
   3.588020769331251*^9, 3.5926615059113026`*^9, 3.619263114163509*^9, 
   3.619263246029581*^9, 3.619263326185244*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["base configuration", "Subsubsection",
 CellChangeTimes->{
  3.504391314503995*^9, {3.513011998415469*^9, 3.513011998449444*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"rfx", " ", ",", "0", ",", "rfz"}], " ", "}"}]}], "]"}], "/.",
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "rc"}]}], " ", "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "Lc", "+", "rt"}]}], " ", "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "/.",
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "Lc", "+", "Lt", "+", "LT", "+", "Lb"}]}], 
        "}"}]}], "]"}], "/.", "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "6"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "Lc", "+", "rt"}]}], " ", "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "7"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", 
         RowBox[{"La", "+", "rc"}]}], " ", "}"}]}], "]"}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "8"]], "[", "0", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"rfx", " ", ",", "0", ",", "rfz"}], " ", "}"}]}], "]"}], "/.",
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.541809370562417*^9, 3.5418093740586166`*^9}, 
   3.541809679358079*^9, {3.5418114374686375`*^9, 3.54181144538809*^9}, {
   3.541859399947259*^9, 3.541859411397679*^9}, {3.541859555463932*^9, 
   3.541859707064998*^9}, {3.541871690391839*^9, 3.5418716942450457`*^9}, {
   3.542729144169736*^9, 3.542729162796169*^9}, {3.542735238561646*^9, 
   3.54273524424045*^9}, {3.542987042644167*^9, 3.542987088187772*^9}, {
   3.5430101802473145`*^9, 3.5430102435789366`*^9}, {3.5430108555009365`*^9, 
   3.543010857449048*^9}, {3.5447123428363557`*^9, 3.5447123660556836`*^9}, {
   3.5452666118953*^9, 3.545266613747406*^9}, {3.54722533616634*^9, 
   3.5472253686982*^9}, {3.54724180077975*^9, 3.547241869871235*^9}, {
   3.5472419070109386`*^9, 3.5472419071439457`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["forward kinematics", "Subsubsection",
 CellChangeTimes->{{3.516466220887102*^9, 3.51646622347725*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "1"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "2"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "3"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "4"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "5"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "6"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "6"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "7"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "7"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", 
     SubscriptBox["sl", "8"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q6"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "8"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5297630988171124`*^9, 3.52976314318359*^9}, {
   3.529763213180913*^9, 3.5297632429457655`*^9}, {3.5297632939734554`*^9, 
   3.529763418617674*^9}, {3.5297640193941355`*^9, 3.529764020354191*^9}, 
   3.529768915904381*^9, {3.5297970280249968`*^9, 3.529797079224286*^9}, 
   3.5298066558203716`*^9, {3.5299344934146757`*^9, 3.5299345140378556`*^9}, {
   3.5314342598205905`*^9, 3.5314342833766317`*^9}, {3.5314367216724205`*^9, 
   3.5314367719669085`*^9}, {3.5315013092709036`*^9, 3.5315013294460573`*^9}, 
   3.531501419253194*^9, {3.531502796964778*^9, 3.531502819366417*^9}, {
   3.5315042029288855`*^9, 3.5315042780051794`*^9}, {3.5315150315305457`*^9, 
   3.5315150701094136`*^9}, {3.531516396790783*^9, 3.531516433887648*^9}, {
   3.5315726721695642`*^9, 3.531572690407607*^9}, {3.5315794306875415`*^9, 
   3.531579468034007*^9}, {3.5351425076223316`*^9, 3.5351425264047647`*^9}, {
   3.5354008027451763`*^9, 3.535400815521599*^9}, {3.5385861328515244`*^9, 
   3.5385861552488055`*^9}, {3.543104332748752*^9, 3.5431044548477354`*^9}, 
   3.5431046912362566`*^9, {3.5431047238661222`*^9, 3.5431047685586786`*^9}, {
   3.54310491631613*^9, 3.543104933117091*^9}, {3.5431052729695296`*^9, 
   3.5431052823050632`*^9}, {3.5472254873509865`*^9, 3.547225487777011*^9}, {
   3.5472265695068827`*^9, 3.5472265860948315`*^9}, {3.5472266409189672`*^9, 
   3.5472266503905087`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", 
     SubscriptBox["sl", "1"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", 
     SubscriptBox["sl", "2"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", 
     SubscriptBox["sl", "3"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", 
     SubscriptBox["sl", "4"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", 
     SubscriptBox["sl", "5"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", 
     SubscriptBox["sl", "6"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "6"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", 
     SubscriptBox["sl", "7"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "7"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", 
     SubscriptBox["sl", "8"]], "[", "\[Theta]", "]"}], "=", 
   RowBox[{
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q6"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", 
         SubscriptBox["sl", "8"]], "[", "0", "]"}]}], "]"}], "]"}], "/.", 
    "constsubs"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.543104505647641*^9, 3.543104512216017*^9}, {
   3.543104551015236*^9, 3.543104579706877*^9}, {3.543104744033276*^9, 
   3.5431047645624504`*^9}, {3.5431049379963703`*^9, 3.543104948377964*^9}, 
   3.543105284531191*^9, 3.543363670019683*^9, 3.5447124567458706`*^9, {
   3.5472268660898457`*^9, 3.547226914427611*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]e", "1"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]e", "2"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]e", "3"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]e", "4"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]e", "5"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]e", "6"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "6"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]e", "7"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "7"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalJ]e", "8"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"BodyJacobian", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "px"], ",", 
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "pz"], ",", 
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q0"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q1"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q2"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q3"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q4"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q5"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["\[Xi]", "q6"], ",", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
      RowBox[{
       SubscriptBox["g", 
        SubscriptBox["sl", "8"]], "[", "0", "]"}]}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Dimensions", "[", 
  SubscriptBox["\[ScriptCapitalJ]e", "8"], "]"}]}], "Input",
 CellChangeTimes->{{3.516708682282812*^9, 3.5167087197409544`*^9}, 
   3.5167087910620337`*^9, 3.516708838777763*^9, {3.5214850806051655`*^9, 
   3.5214850811161947`*^9}, {3.5214874306163054`*^9, 3.521487430743313*^9}, {
   3.5222455685137215`*^9, 3.5222455686177273`*^9}, 3.5222467729536114`*^9, {
   3.541809698454171*^9, 3.5418097079657154`*^9}, {3.541810555006163*^9, 
   3.5418105563442397`*^9}, 3.541810606436105*^9, {3.541811447890233*^9, 
   3.5418114570897593`*^9}, {3.5418129736755033`*^9, 
   3.5418130053613157`*^9}, {3.5418717938980207`*^9, 3.541871805863242*^9}, {
   3.5418718676469517`*^9, 3.5418719657555237`*^9}, {3.5427290095882998`*^9, 
   3.5427290321771393`*^9}, {3.5427290833764296`*^9, 
   3.5427292528707275`*^9}, {3.5431048006935167`*^9, 3.543104824756893*^9}, {
   3.547227154801359*^9, 3.5472271885512896`*^9}, {3.5472282769635434`*^9, 
   3.5472282916083813`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"6", ",", "9"}], "}"}]], "Output",
 CellChangeTimes->{
  3.547228292443429*^9, 3.5472283974264336`*^9, 3.547229298830991*^9, 
   3.547229755854131*^9, 3.5472299652371073`*^9, {3.5472308159667664`*^9, 
   3.547230832687722*^9}, 3.547231788714404*^9, 3.547231914157579*^9, 
   3.547231966935598*^9, 3.547232071372571*^9, 3.5472406963176117`*^9, 
   3.5472409012533665`*^9, 3.5472419580866756`*^9, 3.5473093585411444`*^9, 
   3.5474027497545385`*^9, 3.5474032461419306`*^9, 3.5474035234627924`*^9, 
   3.54749151482161*^9, 3.5477685274802675`*^9, 3.547830416101654*^9, 
   3.5478995704730587`*^9, 3.5478998212334013`*^9, 3.5515414245483503`*^9, 
   3.5526506024165554`*^9, {3.5526529020600877`*^9, 3.5526529213301897`*^9}, 
   3.55282846777218*^9, 3.5529323701730576`*^9, 3.5529325179395094`*^9, 
   3.556317807167967*^9, 3.5563253438948526`*^9, 3.5563273127563705`*^9, 
   3.5563854614278097`*^9, 3.556833543798015*^9, 3.5813374061664*^9, 
   3.5880207694372573`*^9, 3.5926615105175667`*^9, 3.6192631142605667`*^9, 
   3.6192632514688425`*^9, 3.619263327995606*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"Table", "[", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "j"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"ParallelSimplify", "@", 
      RowBox[{"(", 
       RowBox[{"ForwardKinematics", "@@", 
        RowBox[{"Append", "[", 
         RowBox[{
          RowBox[{"Join", "[", 
           RowBox[{
            RowBox[{"{", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{
                SubscriptBox["\[Xi]", "px"], ",", 
                RowBox[{
                 SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
              RowBox[{"{", 
               RowBox[{
                SubscriptBox["\[Xi]", "pz"], ",", 
                RowBox[{
                 SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}]}], "}"}], 
            ",", 
            RowBox[{"Table", "[", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{
                SubscriptBox["\[Xi]", 
                 RowBox[{"If", "[", 
                  RowBox[{
                   RowBox[{"i", "\[LessEqual]", "j"}], ",", 
                   RowBox[{"Symbol", "[", 
                    RowBox[{"\"\<q\>\"", "<>", 
                    RowBox[{"ToString", "[", 
                    RowBox[{"i", "-", "1"}], "]"}]}], "]"}], ",", "0"}], 
                  "]"}]], ",", 
                RowBox[{
                 SubscriptBox["\[Theta]", 
                  RowBox[{"i", "-", "1"}]], "[", "t", "]"}]}], "}"}], ",", 
              RowBox[{"{", 
               RowBox[{"i", ",", 
                RowBox[{"Length", "[", "qe", "]"}]}], "}"}]}], "]"}]}], "]"}],
           ",", 
          RowBox[{
           SubscriptBox["g", 
            SubscriptBox["sl", "j"]], "[", "0", "]"}]}], "]"}]}], ")"}]}]}], 
    ",", 
    RowBox[{"{", 
     RowBox[{"j", ",", 
      RowBox[{"Length", "[", "mm", "]"}]}], "}"}]}], "]"}], ";"}]], "Input",
 CellChangeTimes->{{3.542730612051915*^9, 3.542730879075585*^9}, {
  3.5431046687209682`*^9, 3.543104680134621*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the forward kinematics maps (aniplot)", "Subsubsection",
 CellChangeTimes->{
  3.504391365377686*^9, {3.513012058700656*^9, 3.513012059089004*^9}, {
   3.513013029501931*^9, 3.513013029568491*^9}, {3.5375471566686125`*^9, 
   3.537547158607723*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "sf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "st"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"lt", ",", "0", ",", "0"}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "sh"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", "lh"}], ",", "0", ",", "0"}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "sa"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "La"}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "sk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"La", "+", "Lc"}]}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "ship"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"La", "+", "Lc", "+", "Lt"}]}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"La", "+", "Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "//.",
     "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nship"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"La", "+", "Lc", "+", "Lt"}]}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        RowBox[{"La", "+", "Lc"}]}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsa"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "La"}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsh"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", "lh"}], ",", "0", ",", "0"}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nst"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"lt", ",", "0", ",", "0"}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
   RowBox[{
    RowBox[{"RPToHomogeneous", "[", 
     RowBox[{
      RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "//.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504391344841228*^9, {3.504393660794399*^9, 3.504393663580984*^9}, {
   3.504393700919354*^9, 3.504393704560237*^9}, 3.504477320678662*^9, 
   3.504477372811738*^9, {3.5125106491760826`*^9, 3.5125106821701403`*^9}, {
   3.5125108958281155`*^9, 3.512510897871719*^9}, {3.513012037683773*^9, 
   3.513012046816517*^9}, {3.51353190099301*^9, 3.513531902632749*^9}, {
   3.513875671984485*^9, 3.513875737139051*^9}, {3.515858621125607*^9, 
   3.51585862956809*^9}, {3.5158693201992364`*^9, 3.515869321718323*^9}, {
   3.515886061969276*^9, 3.5158860734820957`*^9}, {3.516708239115464*^9, 
   3.5167082431056924`*^9}, {3.517679572905896*^9, 3.5176795767731175`*^9}, {
   3.5245015787767916`*^9, 3.5245015877623053`*^9}, {3.527424174451967*^9, 
   3.527424174975839*^9}, {3.5277256855926723`*^9, 3.527725689888918*^9}, {
   3.537552677591391*^9, 3.537552730003389*^9}, {3.5375527810023055`*^9, 
   3.5375528292260637`*^9}, {3.53807071887615*^9, 3.5380707952538843`*^9}, {
   3.5381601582699237`*^9, 3.53816017912716*^9}, {3.5382669483592653`*^9, 
   3.5382669513544707`*^9}, {3.5382670857849073`*^9, 3.53826708745411*^9}, 
   3.5382671665004487`*^9, 3.5382672207573442`*^9, 3.538267295497075*^9, 
   3.5382673336547422`*^9, {3.5387330655355225`*^9, 3.5387331383106847`*^9}, {
   3.5387333210411367`*^9, 3.5387333244403305`*^9}, {3.538733615895001*^9, 
   3.538733637920261*^9}, {3.538733693695451*^9, 3.538733718257856*^9}, {
   3.538748011720395*^9, 3.538748023293057*^9}, {3.5399135286464396`*^9, 
   3.5399135385260043`*^9}, {3.5399135876918163`*^9, 
   3.5399135995804963`*^9}, {3.539978631491109*^9, 3.539978650862217*^9}, 
   3.539978774787305*^9, {3.5418099721648264`*^9, 3.5418099895318203`*^9}, {
   3.5418101098707027`*^9, 3.5418101781156063`*^9}, {3.5418102157937613`*^9, 
   3.541810343341057*^9}, {3.541811117321326*^9, 3.5418111264808493`*^9}, {
   3.5418597560334845`*^9, 3.541859803816368*^9}, {3.541862683301814*^9, 
   3.54186270927586*^9}, {3.5425488963617764`*^9, 3.542548912944725*^9}, {
   3.542568102940754*^9, 3.5425681378438025`*^9}, {3.543010250422328*^9, 
   3.543010296241949*^9}, {3.5472273159005737`*^9, 3.547227320468835*^9}, {
   3.5472273768760614`*^9, 3.5472274555005584`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "sf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "sf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "st"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "st"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "sh"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "sh"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "sa"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "sa"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "sk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "sk"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "ship"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "ship"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nship"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nship"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsa"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsa"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsh"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q6"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsh"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nst"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q6"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nst"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q6"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.541870651216957*^9, 3.5418706794686065`*^9}, {
  3.542568156315829*^9, 3.5425681853178697`*^9}, {3.5425682951010237`*^9, 
  3.542568299911031*^9}, {3.5427303424286413`*^9, 3.542730423275984*^9}, {
  3.5427366799522896`*^9, 3.542736697057826*^9}, {3.542737370758452*^9, 
  3.5427374470554304`*^9}, {3.5429003152890663`*^9, 3.542900386955592*^9}, {
  3.5472274683652945`*^9, 3.5472275581494293`*^9}, {3.5472275934664497`*^9, 
  3.547227625046256*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["aniplot", "Subsubsection",
 CellChangeTimes->{{3.51387566509846*^9, 3.513875667385709*^9}, {
  3.513876273981033*^9, 3.51387627490601*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"pos", "=", 
  RowBox[{
   RowBox[{
    RowBox[{"{", 
     RowBox[{
      RowBox[{
       SubscriptBox["g", "sa"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "sh"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "st"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "sa"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "sk"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "ship"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "nship"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "nsa"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "nst"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "nsh"], "[", "\[Theta]", "]"}], ",", 
      RowBox[{
       SubscriptBox["g", "nsa"], "[", "\[Theta]", "]"}]}], "}"}], 
    "\[LeftDoubleBracket]", 
    RowBox[{
     RowBox[{"1", ";;", "All"}], ",", 
     RowBox[{"{", 
      RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], "\[RightDoubleBracket]"}], 
   "\[Transpose]"}]}]], "Input",
 CellChangeTimes->{{3.513875953621905*^9, 3.513876010689337*^9}, {
   3.513876043248599*^9, 3.51387610532577*^9}, {3.513876169675573*^9, 
   3.513876183706996*^9}, {3.513887655923167*^9, 3.513887658548081*^9}, {
   3.513888227999695*^9, 3.513888239050703*^9}, {3.5158587000371203`*^9, 
   3.51585871925922*^9}, {3.5158693371802073`*^9, 3.5158693660608597`*^9}, {
   3.5158696092147675`*^9, 3.5158696155961323`*^9}, {3.515886157784644*^9, 
   3.515886172869871*^9}, {3.516708254867365*^9, 3.5167082553383923`*^9}, {
   3.541810418095332*^9, 3.5418104211185055`*^9}, {3.541811212890792*^9, 
   3.5418112162019815`*^9}, {3.541862754235139*^9, 3.5418627824867887`*^9}, {
   3.5418628258236647`*^9, 3.541862906819007*^9}, {3.5418630211748095`*^9, 
   3.541863023280813*^9}, {3.541863081094515*^9, 3.541863105352557*^9}, {
   3.541863138643016*^9, 3.541863190637907*^9}, {3.5418632874516773`*^9, 
   3.541863352363391*^9}, 3.542472441043125*^9, {3.5429004000128155`*^9, 
   3.542900405457225*^9}, {3.542987196044941*^9, 3.542987200914219*^9}, {
   3.5472276346388044`*^9, 3.5472276839176235`*^9}, 3.5474911332357845`*^9, {
   3.5528284523202963`*^9, 3.5528284531213417`*^9}, {3.55683352413516*^9, 
   3.556833530225739*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"0", ",", 
     RowBox[{"-", 
      FractionBox["127", "2500"]}], ",", 
     FractionBox["381", "2500"], ",", "0", ",", 
     RowBox[{
      RowBox[{"-", 
       FractionBox["254", "625"]}], " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], ",", 
     RowBox[{
      RowBox[{"-", 
       FractionBox["508", "625"]}], " ", 
      RowBox[{"Cos", "[", 
       FractionBox[
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "2"], "]"}], " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        FractionBox[
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "2"]}], "]"}]}], 
     ",", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"-", "4064"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"4064", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"2511", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}]}], 
      "10000"], ",", 
     RowBox[{
      RowBox[{"-", 
       FractionBox["508", "625"]}], " ", 
      RowBox[{"Cos", "[", 
       FractionBox[
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "2"], "]"}], " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        FractionBox[
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "2"]}], "]"}]}], 
     ",", 
     RowBox[{
      RowBox[{"-", 
       FractionBox["254", "625"]}], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], "+", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "-", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], ")"}]}],
      ",", 
     RowBox[{
      RowBox[{"-", 
       FractionBox["254", "625"]}], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], "+", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "-", 
        RowBox[{"2", " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "2"], "]"}], " ", 
         
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           FractionBox[
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "2"]}], 
          "]"}]}]}], ")"}]}], ",", 
     RowBox[{
      FractionBox["1", "2500"], 
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"3", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"8", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
         RowBox[{"8", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"8", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"8", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}], 
        ")"}]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       FractionBox["1", "2500"], 
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}], "+", 
          RowBox[{"8", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"8", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
          
          RowBox[{"8", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
          
          RowBox[{"8", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
          
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}], 
         ")"}]}]}]}], ",", 
     RowBox[{
      RowBox[{"-", 
       FractionBox["254", "625"]}], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], "+", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], "-", 
        RowBox[{"2", " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "2"], "]"}], " ", 
         
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           FractionBox[
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "2"]}], 
          "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox["127", "2500"], ",", "0", ",", "0", ",", 
     FractionBox["127", "2500"], ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{"1", "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}]}], 
        ")"}]}], "2500"], ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{"1", "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}], "2500"], ",", 
     FractionBox[
      RowBox[{"508", "+", 
       RowBox[{"4064", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"4064", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"2511", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}]}], 
      "10000"], ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{"1", "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}], "2500"], ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{"1", "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}], "2500"], ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{"1", "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}], "2500"], ",", 
     RowBox[{"-", 
      RowBox[{
       FractionBox["1", "2500"], 
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"-", "1"}], "-", 
          RowBox[{"8", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
          RowBox[{"8", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"8", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"8", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}], "-", 
          RowBox[{"3", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], 
         ")"}]}]}]}], ",", 
     RowBox[{"-", 
      RowBox[{
       FractionBox["1", "2500"], 
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"-", "1"}], "-", 
          RowBox[{"8", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
          RowBox[{"8", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"8", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"8", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
             RowBox[{
              SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
          
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}], "+", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}], 
         ")"}]}]}]}], ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{"1", "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"8", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
        ")"}]}], "2500"]}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.547491133633807*^9, 3.5474915183578124`*^9, 3.54776853294058*^9, 
   3.5478304197138615`*^9, 3.5478995740462627`*^9, 3.5478998250066166`*^9, 
   3.551541427995956*^9, 3.552650605881754*^9, {3.5526529039101934`*^9, 
   3.5526529231742954`*^9}, 3.5528284712493787`*^9, 3.5529323736582565`*^9, 
   3.552932521430709*^9, 3.5563178140403595`*^9, 3.5563253499131975`*^9, 
   3.556327315474526*^9, 3.556385466309089*^9, 3.556833549355147*^9, 
   3.5813374066014247`*^9, 3.58802076983428*^9, 3.592661514236779*^9, 
   3.6192631143766365`*^9, 3.6192633320324135`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"vtoe", "=", " ", 
  RowBox[{
   SubscriptBox["\[PartialD]", "t"], 
   RowBox[{"pos", "\[LeftDoubleBracket]", 
    RowBox[{"1", ",", "12"}], "\[RightDoubleBracket]"}]}]}]], "Input",
 CellChangeTimes->{{3.547490117955714*^9, 3.5474901814473457`*^9}}],

Cell[BoxData[
 RowBox[{"-", 
  RowBox[{
   FractionBox["1", "2500"], 
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"8", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], " ", 
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "1"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}]}], "+", 
      RowBox[{"8", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", 
      RowBox[{"8", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "3"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "4"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", 
      RowBox[{"8", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "3"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "4"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "5"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", 
      RowBox[{
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "3"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "4"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "5"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "6"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], ")"}]}], "-", 
      RowBox[{
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "1"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "2"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "+", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "3"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "4"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "5"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "-", 
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "6"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}]}], ")"}]}]}], 
     ")"}]}]}]}]], "Output",
 CellChangeTimes->{{3.5474901562969065`*^9, 3.547490182887428*^9}, 
   3.547491518419816*^9, 3.5477685330555863`*^9, 3.5478304198058662`*^9, 
   3.547899574115267*^9, 3.5478998250716205`*^9, 3.5515414280583563`*^9, 
   3.5526506059347568`*^9, {3.5526529039701967`*^9, 3.5526529232342987`*^9}, 
   3.5528284713143826`*^9, 3.55293237371626*^9, 3.552932521482712*^9, 
   3.5563178141233644`*^9, 3.5563253500052023`*^9, 3.5563273155495305`*^9, 
   3.5563854663820934`*^9, 3.5568335494036617`*^9, 3.581337406894441*^9, 
   3.5880207700612926`*^9, 3.592661514288782*^9, 3.6192631146287875`*^9, 
   3.6192633320644197`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"test", " ", "=", " ", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "0"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", "0.2"}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", 
     RowBox[{"-", "0.3"}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", "0.1"}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", "0.4"}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
     RowBox[{"-", "0.2"}]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}], "\[Rule]", "0.1"}]}], 
   "}"}]}]], "Input",
 CellChangeTimes->{{3.542987203468365*^9, 3.5429872568014154`*^9}, {
  3.542987330550634*^9, 3.5429873497577324`*^9}, {3.5446245422125444`*^9, 
  3.544624595365585*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "0"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", "0.2`"}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"-", "0.3`"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", "0.1`"}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", "0.4`"}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"-", "0.2`"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "6"], "[", "t", "]"}], "\[Rule]", "0.1`"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.5429872608326464`*^9, 3.542987353654956*^9, 3.5429875031095037`*^9, 
   3.542998077330315*^9, 3.5429999990605946`*^9, 3.5430103186722317`*^9, 
   3.543010357510453*^9, 3.5430106833370895`*^9, 3.5430108761821194`*^9, 
   3.5430793898460093`*^9, 3.5430796471412535`*^9, 3.543080306534214*^9, 
   3.54310421023037*^9, 3.54310625030543*^9, 3.5432445122912893`*^9, 
   3.5432457695036783`*^9, 3.5432572051053405`*^9, 3.5432573199739103`*^9, 
   3.5432578778938217`*^9, 3.543267360329156*^9, 3.543267465770187*^9, 
   3.5432683174068975`*^9, 3.5432685456889544`*^9, 3.5432687049280624`*^9, 
   3.543268907527651*^9, 3.543269023810302*^9, 3.5433193293424344`*^9, 
   3.5438631297433767`*^9, 3.5438643940821824`*^9, 3.5438644442446713`*^9, 
   3.544545333916089*^9, 3.5445466970810575`*^9, 3.5446216665920687`*^9, 
   3.54462805876468*^9, 3.5446298751115685`*^9, 3.5447124564238524`*^9, 
   3.5452658388120623`*^9, 3.5452666427907753`*^9, 3.5457690213293395`*^9, 
   3.5472213215577173`*^9, 3.547228005066992*^9, 3.5472280598411245`*^9, 
   3.5472283994655504`*^9, 3.54722930353226*^9, 3.5472297608234158`*^9, 
   3.5472299673002253`*^9, {3.547230817978881*^9, 3.5472308347208385`*^9}, 
   3.547231790843526*^9, 3.5472319162827005`*^9, 3.5472319715258603`*^9, 
   3.5472320734936924`*^9, 3.5472406968966436`*^9, 3.54724092696049*^9, 
   3.5472419712411947`*^9, 3.5473093622883587`*^9, 3.5474027536367607`*^9, 
   3.5474032480510397`*^9, 3.547403525318898*^9, 3.5474915184498177`*^9, 
   3.5477685330925884`*^9, 3.547830419845869*^9, 3.5478995741572695`*^9, 
   3.5478998251036224`*^9, 3.5515414280895557`*^9, 3.552650605956758*^9, {
   3.552652903997198*^9, 3.5526529232623005`*^9}, 3.552828471344384*^9, 
   3.5529323737432613`*^9, 3.5529325214977126`*^9, 3.556317814182368*^9, 
   3.556325350081207*^9, 3.5563273156025333`*^9, 3.556385466438097*^9, 
   3.556833549434307*^9, 3.581337406976446*^9, 3.5880207701422977`*^9, 
   3.5926615142997828`*^9, 3.619263114656804*^9, 3.6192633320764217`*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"testval", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "->", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.221239332041986", ",", "0.101780805101294", ",", 
           RowBox[{"-", "0.637383872483534"}], ",", "0.159314134330659", ",", 
           "0.539214942189742", ",", "0.147629355500069"}], "}"}], "[", 
         RowBox[{"[", "i", "]"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "6"}], "}"}]}], "]"}], ",", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "i"], "'"}], "[", "t", "]"}], "->", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0.123802245355501", ",", 
           RowBox[{"-", "0.118351579433827"}], ",", 
           RowBox[{"-", "2.351151470443853"}], ",", "3.129536379532168", ",", 
           
           RowBox[{"-", "0.722226294284023"}], ",", "0.133219859731242"}], 
          "}"}], "[", 
         RowBox[{"[", "i", "]"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "6"}], "}"}]}], "]"}]}], "]"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.5173209584749875`*^9, 3.5173210724245048`*^9}, {
   3.517321247432515*^9, 3.5173212529188285`*^9}, {3.5173232113088417`*^9, 
   3.517323221924449*^9}, {3.53072272280212*^9, 3.5307227229113207`*^9}, {
   3.5307230525866995`*^9, 3.5307230526802998`*^9}, {3.5307239802124434`*^9, 
   3.530723980399644*^9}, 3.5307258430727243`*^9, 3.530727094648729*^9, {
   3.5307278417284417`*^9, 3.530727841900042*^9}, 3.5308046591524487`*^9, 
   3.530805289740516*^9, 3.530807135248373*^9, {3.5314339339952164`*^9, 
   3.531433973369685*^9}, {3.5314342002752857`*^9, 3.5314342376685514`*^9}, {
   3.531437916673567*^9, 3.531437918985699*^9}, {3.5314396852237225`*^9, 
   3.5314396935671997`*^9}, {3.5315040168762436`*^9, 
   3.5315040199644203`*^9}, {3.53151498825607*^9, 3.5315149970856853`*^9}, {
   3.531516325763858*^9, 3.531516339367082*^9}, {3.531572633028325*^9, 
   3.531572642411862*^9}, {3.5366081769470234`*^9, 3.536608211122978*^9}, {
   3.5366317933643026`*^9, 3.5366318873396773`*^9}, {3.536633723190682*^9, 
   3.5366337235187006`*^9}, {3.5385868256671515`*^9, 
   3.5385868456502943`*^9}, {3.543105585184387*^9, 3.5431056650909576`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the manipulator inertia matrix", "Subsubsection",
 CellChangeTimes->{
  3.504392305735899*^9, 3.513012096077317*^9, {3.516708802974715*^9, 
   3.5167088033737383`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalD]e", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{
      UnderoverscriptBox["\[Sum]", 
       RowBox[{"i", "=", "1"}], 
       RowBox[{"Length", "[", "mm", "]"}]], 
      RowBox[{
       RowBox[{
        SubscriptBox["\[ScriptCapitalJ]e", "i"], "\[Transpose]"}], ".", 
       SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
       SubscriptBox["\[ScriptCapitalJ]e", "i"]}]}], "+", 
     RowBox[{"DiagonalMatrix", "[", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", 
        SubscriptBox["Mt", "1"], ",", 
        SubscriptBox["Mt", "2"], ",", 
        SubscriptBox["Mt", "3"], ",", 
        SubscriptBox["Mt", "4"], ",", 
        SubscriptBox["Mt", "6"], ",", 
        SubscriptBox["Mt", "7"], ",", 
        SubscriptBox["Mt", "8"]}], "}"}], "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5167096143551235`*^9, 3.516709632103139*^9}, 
   3.541811482606219*^9, {3.5427304345236034`*^9, 3.542730437924409*^9}, {
   3.5429004219776535`*^9, 3.5429004251444592`*^9}, {3.5431048322143197`*^9, 
   3.543104834414446*^9}, {3.5472277974041142`*^9, 3.5472277979491453`*^9}, {
   3.5472279040282125`*^9, 3.547227933050873*^9}, {3.5472283715309525`*^9, 
   3.5472283773762865`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"\[ScriptCapitalD]e", "/.", "test"}], "/.", "p0"}], "//", 
  "N"}]], "Input",
 CellChangeTimes->{{3.5432683063002625`*^9, 3.5432683083963823`*^9}, {
  3.5432684711006885`*^9, 3.5432684725407705`*^9}, {3.54454531225385*^9, 
  3.5445453155650396`*^9}, {3.5446296048661118`*^9, 3.544629605762163*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"31.700172301129918`", ",", "0.013338619693579938`", ",", 
     RowBox[{"-", "22.797413725385102`"}], ",", 
     RowBox[{"-", "21.2376084024877`"}], ",", 
     RowBox[{"-", "9.776123297249777`"}], ",", "0.29550322808602575`", ",", 
     RowBox[{"-", "3.7953180288997492`"}], ",", 
     RowBox[{"-", "1.2529955375838309`"}], ",", 
     RowBox[{"-", "0.08725495140772163`"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"0.013338619693579938`", ",", "30.490804182467343`", ",", 
     RowBox[{"-", "2.3471201517579967`"}], ",", 
     RowBox[{"-", "2.4782435598775625`"}], ",", 
     RowBox[{"-", "0.24743448987390315`"}], ",", 
     RowBox[{"-", "1.2035615533230943`"}], ",", "1.2002122259180363`", ",", 
     "0.12533551674415705`", ",", 
     RowBox[{"-", "0.11097179887350271`"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "22.797413725385102`"}], ",", 
     RowBox[{"-", "2.3471201517579967`"}], ",", "21.774948151325052`", ",", 
     "20.59311169995969`", ",", "11.48197172879199`", ",", 
     "3.2229960322567024`", ",", "0.7904671616733482`", ",", 
     "0.14434001249470968`", ",", "0.011737675510211142`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "21.2376084024877`"}], ",", 
     RowBox[{"-", "2.4782435598775625`"}], ",", "20.59311169995969`", ",", 
     "19.53402274651137`", ",", "10.985344665291704`", ",", 
     "3.2380075962434725`", ",", "0.5976650058052412`", ",", 
     "0.08068783918545105`", ",", "0.007305123978698881`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "9.776123297249777`"}], ",", 
     RowBox[{"-", "0.24743448987390315`"}], ",", "11.48197172879199`", ",", 
     "10.985344665291702`", ",", "7.091327859252712`", ",", 
     "3.2585316371346327`", ",", 
     RowBox[{"-", "0.8171023928149348`"}], ",", 
     RowBox[{"-", "0.4082596107901715`"}], ",", 
     RowBox[{"-", "0.03640821694910734`"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"0.2955032280860257`", ",", 
     RowBox[{"-", "1.2035615533230943`"}], ",", "3.222996032256703`", ",", 
     "3.238007596243473`", ",", "3.2585316371346327`", ",", 
     "3.4466370011281686`", ",", 
     RowBox[{"-", "2.400509349245785`"}], ",", 
     RowBox[{"-", "0.9200181816156942`"}], ",", 
     RowBox[{"-", "0.06718909366869517`"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "3.7953180288997492`"}], ",", "1.2002122259180363`", ",", 
     "0.7904671616733482`", ",", "0.5976650058052412`", ",", 
     RowBox[{"-", "0.8171023928149348`"}], ",", 
     RowBox[{"-", "2.400509349245785`"}], ",", "2.420290902643843`", ",", 
     "0.9200181816156942`", ",", "0.06718909366869517`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "1.2529955375838309`"}], ",", "0.12533551674415705`", ",", 
     "0.14434001249470968`", ",", "0.08068783918545105`", ",", 
     RowBox[{"-", "0.4082596107901715`"}], ",", 
     RowBox[{"-", "0.9200181816156942`"}], ",", "0.9200181816156942`", ",", 
     "0.45094391232458964`", ",", "0.052090245201503106`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "0.08725495140772163`"}], ",", 
     RowBox[{"-", "0.11097179887350271`"}], ",", "0.011737675510211142`", ",",
      "0.007305123978698881`", ",", 
     RowBox[{"-", "0.03640821694910734`"}], ",", 
     RowBox[{"-", "0.06718909366869517`"}], ",", "0.06718909366869517`", ",", 
     "0.052090245201503106`", ",", "0.0460780097578608`"}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.544712462252186*^9, 3.5452658444003773`*^9, 3.5452666482489834`*^9, 
   3.5457690276677017`*^9, 3.5472213336804104`*^9, 3.547228085146572*^9, 
   3.5472284024687223`*^9, 3.5472293150469184`*^9, 3.5472297724350796`*^9, 
   3.5472299698393707`*^9, {3.5472308190419416`*^9, 3.547230835075859*^9}, 
   3.547231815416931*^9, 3.547231929750471*^9, 3.547231996562292*^9, 
   3.547232086949462*^9, 3.547240710143386*^9, 3.5472410990819583`*^9, 
   3.547242024197878*^9, 3.547309367953683*^9, 3.547402759691107*^9, 
   3.547403249554126*^9, 3.5474035256179156`*^9, 3.5474915241641445`*^9, 
   3.547768547049387*^9, 3.547830425162173*^9, 3.5478995794295707`*^9, 
   3.547899830968958*^9, 3.5515414333623657`*^9, 3.552650611091052*^9, 
   3.552652928661609*^9, 3.5528284797808666`*^9, 3.552932381980733*^9, 
   3.552932530348219*^9, 3.5563178365366464`*^9, 3.5563253712614183`*^9, 
   3.556327316734598*^9, 3.5563854834900723`*^9, 3.556833566713951*^9, 
   3.581337410981675*^9, 3.588020773841509*^9, 3.5926615231862907`*^9, 
   3.6192631204432745`*^9, 3.619263343883783*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalD]", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
      RowBox[{
       RowBox[{"4", ";;", "All"}], ",", 
       RowBox[{"4", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.513012422937284*^9, 3.513012423523879*^9}, {
   3.513012453927046*^9, 3.513012465021281*^9}, {3.513012554051636*^9, 
   3.513012587164631*^9}, {3.51353194017507*^9, 3.513531940256729*^9}, 
   3.515858726761649*^9, {3.515859543127342*^9, 3.5158595437243767`*^9}, 
   3.5158680079901824`*^9, 3.5417801424043016`*^9, 3.541811484405322*^9, 
   3.54187175917236*^9, 3.541872003039589*^9, {3.542728967000225*^9, 
   3.5427289712746325`*^9}, {3.542987412594327*^9, 3.542987414593441*^9}, 
   3.542987449728451*^9, 3.5430103409065037`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate Coriolis matrix", "Subsubsection",
 CellChangeTimes->{{3.504392477990152*^9, 3.504392495055983*^9}, 
   3.513012592090603*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalC]e", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"InertiaToCoriolis", "[", 
     RowBox[{"\[ScriptCapitalD]e", ",", 
      RowBox[{"Flatten", "[", "qe", "]"}], ",", 
      RowBox[{"Flatten", "[", "dqe", "]"}]}], "]"}], "]"}]}], ";"}]], "Input",\

 CellChangeTimes->{
  3.50439255557097*^9, {3.51295513827149*^9, 3.512955172679001*^9}, {
   3.5129552037364693`*^9, 3.512955231006535*^9}, {3.512955263739307*^9, 
   3.5129553328924294`*^9}, {3.512992901952466*^9, 3.512992920307856*^9}, {
   3.5129940359160023`*^9, 3.512994058469393*^9}, 3.5129940928271093`*^9, {
   3.512994138765036*^9, 3.5129941583899803`*^9}, {3.51301260617554*^9, 
   3.513012607311402*^9}, {3.513012641937742*^9, 3.513012652342953*^9}, {
   3.513012738154763*^9, 3.513012748062248*^9}, 3.513012780026892*^9, {
   3.51301313424288*^9, 3.513013136853251*^9}, 3.541811486093418*^9, 
   3.5427300155608673`*^9, {3.5427304454904222`*^9, 3.5427304492656293`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Simplify", "::", "time"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Time spent on a transformation exceeded \\!\\(300.`\\) \
seconds, and the transformation was aborted. Increasing the value of \
TimeConstraint option may improve the result of simplification.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263955742835*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Simplify", "::", "time"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Time spent on a transformation exceeded \\!\\(300.`\\) \
seconds, and the transformation was aborted. Increasing the value of \
TimeConstraint option may improve the result of simplification.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192642574500027`*^9}],

Cell[BoxData["$Aborted"], "Output",
 CellChangeTimes->{3.619264550533308*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[ScriptCapitalC]", "=", 
   RowBox[{
    RowBox[{"\[ScriptCapitalC]e", "\[LeftDoubleBracket]", 
     RowBox[{
      RowBox[{"4", ";;", "All"}], ",", 
      RowBox[{"4", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
    "\[InvisibleSpace]", "p0"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5427304545696383`*^9, 3.5427304822596874`*^9}, 
   3.5427345740606112`*^9, {3.5427346218981485`*^9, 3.542734648186597*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the potential energy and \[ScriptCapitalG] matrix", \
"Subsubsection",
 CellChangeTimes->{
  3.50439250607954*^9, {3.512994059885461*^9, 3.512994061579727*^9}, {
   3.513013138038511*^9, 3.513013138090329*^9}, 3.51301482840405*^9}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"V", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"g", " ", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        " ", 
        RowBox[{
         RowBox[{
          SubscriptBox["ge", 
           SubscriptBox["sl", "i"]], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"\[ScriptCapitalG]e", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{"qe", ",", "1"}], "}"}]], "V"}], "]"}]}], ";", 
  RowBox[{
   RowBox[{"Needs", "[", "\"\<Units`\>\"", "]"}], 
   "q"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"\[ScriptCapitalG]", "  ", "=", " ", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]e", "\[LeftDoubleBracket]", 
     RowBox[{"4", ";;", "All"}], "\[RightDoubleBracket]"}], "/.", 
    "\[InvisibleSpace]", "p0"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.504392530579445*^9, 3.504392568536552*^9}, {
   3.5052280833869843`*^9, 3.5052280856333885`*^9}, {3.5125114305562534`*^9, 
   3.512511430665453*^9}, {3.512994063656911*^9, 3.512994082683117*^9}, {
   3.513013142091812*^9, 3.513013228804931*^9}, {3.5158498432526364`*^9, 
   3.5158498449907355`*^9}, {3.5418114872784863`*^9, 
   3.5418114888695774`*^9}, {3.542730572661846*^9, 3.5427305803526597`*^9}, {
   3.5427309211020584`*^9, 3.542730929463673*^9}, {3.5427309623953314`*^9, 
   3.542730994718588*^9}, {3.542734955523368*^9, 3.5427349851129665`*^9}, 
   3.5427350172735047`*^9, {3.5429873023910236`*^9, 3.5429873073023043`*^9}, {
   3.5429873634135137`*^9, 3.5429874025947547`*^9}, {3.5431048649021893`*^9, 
   3.5431049044114494`*^9}, {3.5431049615367165`*^9, 3.54310502348626*^9}, 
   3.543105337737234*^9, {3.5431054027339516`*^9, 3.5431054043180428`*^9}, {
   3.543105460843275*^9, 3.5431054637794433`*^9}, {3.5431056787297373`*^9, 
   3.543105745399551*^9}, {3.5431058015327616`*^9, 3.543105816435614*^9}, {
   3.5472319369828844`*^9, 3.547231937453912*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(4\\)\[NoBreak] of \
\[NoBreak]\\!\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\
\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631218851385`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(4\\)\[NoBreak] of \
\[NoBreak]\\!\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\
\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631219141564`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(4\\)\[NoBreak] of \
\[NoBreak]\\!\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\
\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631219451747`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \[NoBreak]\\!\\(\\*StyleBox[\\(Part :: \
partw\\), \\\"MessageName\\\"]\\)\[NoBreak] will be suppressed during this \
calculation. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/stop\\\", ButtonNote -> \
\\\"General::stop\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263121965187*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\(#2\\)\[NoBreak] cannot be \
used as a part specification. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, ButtonData:>\\\"paclet:ref/Part\
\\\", ButtonNote -> \\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631220262237`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\(#2\\)\[NoBreak] cannot be \
used as a part specification. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, ButtonData:>\\\"paclet:ref/Part\
\\\", ButtonNote -> \\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.61926312207025*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\(#2\\)\[NoBreak] cannot be \
used as a part specification. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, ButtonData:>\\\"paclet:ref/Part\
\\\", ButtonNote -> \\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631221162777`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \[NoBreak]\\!\\(\\*StyleBox[\\(Part :: \
pkspec1\\), \\\"MessageName\\\"]\\)\[NoBreak] will be suppressed during this \
calculation. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/stop\\\", ButtonNote -> \
\\\"General::stop\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631221362896`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122269369*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631222773743`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122284378*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631222913823`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122312395*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631223204*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122327404*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631223344083`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122355421*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631223634253`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.61926312237043*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122378435*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \\!\\(\\*StyleBox[\\(Part :: pkspec1\\), \
\\\"MessageName\\\"]\\) will be suppressed during this calculation.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631223994474`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \\!\\(\\*StyleBox[\\(Part :: pkspec1\\), \
\\\"MessageName\\\"]\\) will be suppressed during this calculation.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122407452*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \\!\\(\\*StyleBox[\\(Part :: pkspec1\\), \
\\\"MessageName\\\"]\\) will be suppressed during this calculation.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631224144564`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \\!\\(\\*StyleBox[\\(Part :: pkspec1\\), \
\\\"MessageName\\\"]\\) will be suppressed during this calculation.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631224224615`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631225635457`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631225815563`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122598567*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631226165776`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631226365895`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631226556005`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122674612*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122692623*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122710634*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631227296453`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631227466555`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \\!\\(#2\\) cannot be used as a part \
specification.\"\>"}]], "Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631227626653`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \\!\\(\\*StyleBox[\\(Part :: pkspec1\\), \
\\\"MessageName\\\"]\\) will be suppressed during this calculation.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631227796755`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \\!\\(\\*StyleBox[\\(Part :: pkspec1\\), \
\\\"MessageName\\\"]\\) will be suppressed during this calculation.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631227966857`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \\!\\(\\*StyleBox[\\(Part :: pkspec1\\), \
\\\"MessageName\\\"]\\) will be suppressed during this calculation.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.6192631228146963`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \\!\\(\\*StyleBox[\\(Part :: pkspec1\\), \
\\\"MessageName\\\"]\\) will be suppressed during this calculation.\"\>"}]], \
"Message", "MSG",
 ShowCellLabel->True,
 CellChangeTimes->{3.619263122831706*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"Null", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"Null", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"Null", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"Null", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"Null", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"Null", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{3.5472322544310417`*^9, 3.547240714820612*^9, 
  3.547242268566478*^9, 3.5473093997214994`*^9, 3.5474027895428143`*^9, 
  3.547403255739479*^9, 3.5474035278410425`*^9, 3.5474915516347156`*^9, 
  3.547768628976073*^9, 3.547830454553854*^9, 3.547899859098567*^9, 
  3.5515414594144115`*^9, 3.552650637149542*^9, 3.5526529834427423`*^9, 
  3.552828542125433*^9, 3.5529325990341473`*^9, 3.5563180270025406`*^9, 
  3.55632552096564*^9, 3.556327333845577*^9, 3.556385641029931*^9, 
  3.556833698156604*^9, 3.5813374122657485`*^9, 3.5880207750935802`*^9, 
  3.5926615913611903`*^9, 3.6192631228527193`*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{"V", "=", 
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{
      RowBox[{"g", " ", 
       RowBox[{
        UnderoverscriptBox["\[Sum]", 
         RowBox[{"i", "=", "1"}], 
         RowBox[{"Length", "[", "mm", "]"}]], 
        RowBox[{
         RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}],
          " ", 
         RowBox[{
          RowBox[{
           SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";", "\[IndentingNewLine]", 
   RowBox[{"\[ScriptCapitalG]e", "=", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{"ParallelSimplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{"qe", ",", "1"}], "}"}]], "V"}], "]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "/.", 
      RowBox[{"testval", "\[IndentingNewLine]", "\[ScriptCapitalG]"}]}], "  ",
      "=", " ", 
     RowBox[{
      RowBox[{"\[ScriptCapitalG]e", "\[LeftDoubleBracket]", 
       RowBox[{"4", ";;", "All"}], "\[RightDoubleBracket]"}], "/.", 
      "\[InvisibleSpace]", "p0"}]}]}], ";"}], "*)"}]], "Input",
 CellChangeTimes->{{3.5431050075343475`*^9, 3.5431050349019127`*^9}, {
  3.5431057144167786`*^9, 3.543105756597192*^9}, {3.5431058200448203`*^9, 
  3.54310582265797*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Calc. J for lagrange multipliers", "Subsubsection",
 CellChangeTimes->{{3.537573191239087*^9, 3.537573202565735*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"CrossProd", "[", "\[CapitalOmega]_", "]"}], ":=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"\[CapitalOmega]", "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "2"}], "\[RightDoubleBracket]"}], ",", 
     RowBox[{"\[CapitalOmega]", "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "3"}], "\[RightDoubleBracket]"}], ",", 
     RowBox[{"\[CapitalOmega]", "\[LeftDoubleBracket]", 
      RowBox[{"2", ",", "1"}], "\[RightDoubleBracket]"}]}], "}"}]}], 
  ";"}]], "Input"],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{
    SubscriptBox["p", "ht"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{"RigidPosition", "[", 
       RowBox[{
        SubscriptBox["g", "ht"], "[", "\[Theta]", "]"}], "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"{", 
       RowBox[{"1", ",", "3"}], "}"}], "\[RightDoubleBracket]"}], "//", 
     "ParallelSimplify"}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["p", "hh"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{"RigidPosition", "[", 
       RowBox[{
        SubscriptBox["g", "hh"], "[", "\[Theta]", "]"}], "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"{", 
       RowBox[{"1", ",", "3"}], "}"}], "\[RightDoubleBracket]"}], "//", 
     "ParallelSimplify"}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["p", "pt"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{"RigidPosition", "[", 
       RowBox[{
        SubscriptBox["g", "pt"], "[", "\[Theta]", "]"}], "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"{", 
       RowBox[{"1", ",", "3"}], "}"}], "\[RightDoubleBracket]"}], "//", 
     "ParallelSimplify"}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["p", "ph"], "=", 
    RowBox[{
     RowBox[{
      RowBox[{"RigidPosition", "[", 
       RowBox[{
        SubscriptBox["g", "ph"], "[", "\[Theta]", "]"}], "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"{", 
       RowBox[{"1", ",", "3"}], "}"}], "\[RightDoubleBracket]"}], "//", 
     "ParallelSimplify"}]}], ";"}], "*)"}]], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.5375588753168807`*^9, 3.5375588899817195`*^9}, {
   3.537573174740144*^9, 3.5375731773452926`*^9}, {3.5375732102091722`*^9, 
   3.5375732118012633`*^9}, {3.537573261374099*^9, 3.5375733077747526`*^9}, 
   3.5375733449958816`*^9, {3.5375736518074303`*^9, 3.537573658655822*^9}, {
   3.5375737216524253`*^9, 3.5375737568484383`*^9}, {3.5375739551217785`*^9, 
   3.537574147565786*^9}, {3.537574299322466*^9, 3.5375744607246976`*^9}, {
   3.538267055442854*^9, 3.5382670620572653`*^9}, 3.538318067030096*^9, 
   3.5383184570015817`*^9, {3.5387338682614355`*^9, 3.5387338777349777`*^9}, {
   3.5387483814685435`*^9, 3.538748383657669*^9}, {3.539113656699425*^9, 
   3.539113673578655*^9}, {3.542548555091257*^9, 3.5425485554432774`*^9}, 
   3.542549112563143*^9, 3.54254918017801*^9, {3.542549215462028*^9, 
   3.5425492390133753`*^9}, 3.5425493198900013`*^9, {3.5425686137514715`*^9, 
   3.5425686278434916`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", "sf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "sf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["ge", "nsf"], "[", "\[Theta]", "]"}], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{"ForwardKinematics", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "px"], ",", 
         RowBox[{
          SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "pz"], ",", 
         RowBox[{
          SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q0"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q1"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q2"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q3"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q4"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q5"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Xi]", "q6"], ",", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
       RowBox[{
        SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
     "constsubs"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5427319532782717`*^9, 3.542731958660281*^9}, {
  3.5429004410720873`*^9, 3.5429004435212917`*^9}, {3.5429004752049475`*^9, 
  3.542900496327384*^9}, {3.543079293047636*^9, 3.5430793479810133`*^9}, {
  3.54308018515134*^9, 3.5430802725384417`*^9}, {3.543105919601515*^9, 
  3.5431059304651365`*^9}, {3.547228738592947*^9, 3.547228743377221*^9}, {
  3.5472287818334208`*^9, 3.5472288160323763`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["pe", "sf"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{"RigidPosition", "[", 
      RowBox[{
       SubscriptBox["ge", "sf"], "[", "\[Theta]", "]"}], "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"{", 
      RowBox[{"1", ",", "3"}], "}"}], "\[RightDoubleBracket]"}], "//", 
    "ParallelSimplify"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["Re", "sf"], "=", 
   RowBox[{"RigidOrientation", "[", 
    RowBox[{
     SubscriptBox["ge", "sf"], "[", "\[Theta]", "]"}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Omega]e", "sf"], "=", 
   RowBox[{
    RowBox[{"CrossProd", "[", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Inverse", "[", 
        SubscriptBox["Re", "sf"], "]"}], ".", 
       RowBox[{
        SubscriptBox["\[PartialD]", "t"], 
        SubscriptBox["Re", "sf"]}]}], "]"}], "]"}], "\[LeftDoubleBracket]", 
    "2", "\[RightDoubleBracket]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.542568633083499*^9, 3.542568658775535*^9}, {
  3.542731802509207*^9, 3.542731810574421*^9}, {3.542732615062634*^9, 
  3.542732623159048*^9}, {3.5429004463448963`*^9, 3.5429004657981305`*^9}, {
  3.5429005055002003`*^9, 3.54290052262903*^9}, {3.542986592086396*^9, 
  3.542986603310038*^9}, {3.5429874615041237`*^9, 3.5429874656003585`*^9}, {
  3.5430793134900723`*^9, 3.543079364181055*^9}, {3.543105934974394*^9, 
  3.5431059501732635`*^9}, {3.547228704799014*^9, 3.5472287316215487`*^9}, {
  3.547228868229362*^9, 3.5472288860523815`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\({1, 3}\\)\[NoBreak] of \
\[NoBreak]\\!\\(RigidPosition[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263122929765*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\({1, 3}\\)\[NoBreak] of \
\[NoBreak]\\!\\(RigidPosition[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631229617844`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(3\\)\[NoBreak] of \[NoBreak]\\!\\(\\(\
\\(Inverse[\\(\\(RigidOrientation[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\\)]\\)\\) . \
\\(\\((\\(\\(\\(\\(\\*SuperscriptBox[\\\"RigidOrientation\\\", \\\"\[Prime]\\\
\", Rule[MultilineFunction, \
None]]\\)\\)[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(PrismaticTwist[\\(\\(\
\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, 0}\\)\\)\\)\\)]\\)\\), \
\\(\\(p\\_x[t]\\)\\)}\\)\\), \\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\\)\\\\ \\(\\((\\(\[LeftSkeleton] 1 \[RightSkeleton]\\))\\)\\))\
\\)\\)\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\
\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123000808*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(3\\)\[NoBreak] of \
\[NoBreak]\\!\\(Inverse[\\(\\(RigidOrientation[\\(\\(ForwardKinematics[\\(\\(\
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631230328274`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(3\\)\[NoBreak] of \
\[NoBreak]\\!\\(Inverse[\\(\\(RigidOrientation[\\(\\(ForwardKinematics[\\(\\(\
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631230648465`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \[NoBreak]\\!\\(\\*StyleBox[\\(Part :: \
partw\\), \\\"MessageName\\\"]\\)\[NoBreak] will be suppressed during this \
calculation. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/stop\\\", ButtonNote -> \
\\\"General::stop\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631230808563`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["pe", "nsf"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{"RigidPosition", "[", 
      RowBox[{
       SubscriptBox["ge", "nsf"], "[", "\[Theta]", "]"}], "]"}], 
     "\[LeftDoubleBracket]", 
     RowBox[{"{", 
      RowBox[{"1", ",", "3"}], "}"}], "\[RightDoubleBracket]"}], "//", 
    "ParallelSimplify"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["Re", "nsf"], "=", 
   RowBox[{
    RowBox[{"RigidOrientation", "[", 
     RowBox[{
      SubscriptBox["ge", "nsf"], "[", "\[Theta]", "]"}], "]"}], "//", 
    "ParallelSimplify"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[Omega]e", "nsf"], "=", 
   RowBox[{
    RowBox[{"CrossProd", "[", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Inverse", "[", 
        SubscriptBox["Re", "nsf"], "]"}], ".", 
       RowBox[{
        SubscriptBox["\[PartialD]", "t"], 
        SubscriptBox["Re", "nsf"]}]}], "]"}], "]"}], "\[LeftDoubleBracket]", 
    "2", "\[RightDoubleBracket]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.542568633083499*^9, 3.542568658775535*^9}, {
  3.542731802509207*^9, 3.542731810574421*^9}, {3.542732615062634*^9, 
  3.542732623159048*^9}, {3.5429004463448963`*^9, 3.5429004657981305`*^9}, {
  3.5429005055002003`*^9, 3.54290052262903*^9}, {3.542986592086396*^9, 
  3.542986603310038*^9}, {3.5429874615041237`*^9, 3.5429874656003585`*^9}, {
  3.5430793134900723`*^9, 3.543079321124758*^9}, {3.543079458965248*^9, 
  3.543079472168316*^9}, {3.54310595534956*^9, 3.543105969277356*^9}, {
  3.547228890901659*^9, 3.547228908964692*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\({1, 3}\\)\[NoBreak] of \
\[NoBreak]\\!\\(RigidPosition[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_1[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_2[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_3[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_4[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_5[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, 1, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_6[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123117878*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\({1, 3}\\)\[NoBreak] of \
\[NoBreak]\\!\\(RigidPosition[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_1[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_2[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_3[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_4[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_5[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, 1, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_6[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123149897*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(3\\)\[NoBreak] of \[NoBreak]\\!\\(\\(\
\\(Inverse[\\(\\(RigidOrientation[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_1[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_2[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_3[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_4[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_5[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, 1, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_6[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\\)]\\)\\) . \\(\\((\\(\[LeftSkeleton] 1 \
\[RightSkeleton]\\))\\)\\)\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\
\"\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631232169375`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(3\\)\[NoBreak] of \
\[NoBreak]\\!\\(Inverse[\\(\\(RigidOrientation[\\(\\(ForwardKinematics[\\(\\(\
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_1[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_2[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_3[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_4[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_5[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, 1, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_6[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123249957*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(3\\)\[NoBreak] of \
\[NoBreak]\\!\\(Inverse[\\(\\(RigidOrientation[\\(\\(ForwardKinematics[\\(\\(\
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_1[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_2[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_3[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_4[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_5[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, 1, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_6[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631232819767`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \[NoBreak]\\!\\(\\*StyleBox[\\(Part :: \
partw\\), \\\"MessageName\\\"]\\)\[NoBreak] will be suppressed during this \
calculation. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/stop\\\", ButtonNote -> \
\\\"General::stop\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123298987*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   SubscriptBox["J", "d"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"Insert", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Join", "[", 
          RowBox[{
           SubscriptBox["p", "ht"], ",", 
           SubscriptBox["p", "pt"]}], "]"}]}], ",", 
        RowBox[{
         SubscriptBox["J", "\[Omega]"], "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"2", ";;", "2"}], ",", 
          RowBox[{"1", ";;", "All"}]}], "\[RightDoubleBracket]"}]}], "]"}], 
      ",", 
      RowBox[{
       SubscriptBox["J", "\[Omega]"], "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", 
        RowBox[{"1", ";;", "All"}]}], "\[RightDoubleBracket]"}], ",", "3"}], 
     "]"}], "]"}]}], "*)"}]], "Input",
 CellChangeTimes->{{3.5376210599855905`*^9, 3.537621163202494*^9}, {
   3.537621421677278*^9, 3.53762142782863*^9}, 3.5376215357318015`*^9, {
   3.5376266073888836`*^9, 3.5376266093399954`*^9}, {3.5378190300160933`*^9, 
   3.537819035352399*^9}, {3.5380785353379326`*^9, 3.5380785563811364`*^9}, {
   3.538078708866858*^9, 3.538078712201049*^9}, 3.538080110757042*^9, {
   3.5381411039395285`*^9, 3.5381411079175353`*^9}, {3.538141411152473*^9, 
   3.538141415582881*^9}, 3.538142643492941*^9, {3.538318168447875*^9, 
   3.5383182355435925`*^9}, {3.538318271985256*^9, 3.538318292639693*^9}, {
   3.538318327614954*^9, 3.5383183641346183`*^9}, {3.5383184054122906`*^9, 
   3.538318419218315*^9}, {3.5383187022624125`*^9, 3.538318702465213*^9}, {
   3.538318748376094*^9, 3.5383188020713882`*^9}, {3.5386883195016823`*^9, 
   3.5386883222518396`*^9}, {3.5387339084417334`*^9, 3.538733915846157*^9}, {
   3.538748387294877*^9, 3.538748388771961*^9}, {3.5387630066550565`*^9, 
   3.5387630194987907`*^9}, {3.538820547512205*^9, 3.538820547685215*^9}, 
   3.5402484934353333`*^9, 3.5418121129862747`*^9, 3.5418127230231667`*^9, {
   3.5418740947783813`*^9, 3.5418741478496747`*^9}, {3.5418742451626453`*^9, 
   3.5418742468318486`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["J", "\[Omega]"], "=", 
  RowBox[{
   SubscriptBox["\[PartialD]", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"Flatten", "[", "dqe", "]"}], ",", "1"}], "}"}]], 
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Omega]e", "sf"], ",", 
     SubscriptBox["\[Omega]e", "nsf"]}], "}"}]}]}]], "Input",
 CellChangeTimes->{{3.5430794850716047`*^9, 3.543079487427209*^9}, 
   3.5430795431564555`*^9, 3.5430796212727423`*^9, 3.5430802262122746`*^9, {
   3.5431062193136573`*^9, 3.5431062370316706`*^9}, {3.5472289176681895`*^9, 
   3.5472289206673613`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\(3\\)\[NoBreak] of \
\[NoBreak]\\!\\(Inverse[\\(\\(RigidOrientation[\\(\\(ForwardKinematics[\\(\\(\
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631233520184`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\(#2\\)\[NoBreak] cannot be \
used as a part specification. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, ButtonData:>\\\"paclet:ref/Part\
\\\", ButtonNote -> \\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631233970456`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\(#2\\)\[NoBreak] cannot be \
used as a part specification. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, ButtonData:>\\\"paclet:ref/Part\
\\\", ButtonNote -> \\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631234420724`*^9}],

Cell[BoxData[
 InterpretationBox[
  TagBox[
   FrameBox[GridBox[{
      {
       ItemBox[
        TagBox[
         RowBox[{"{", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{
             RowBox[{
              RowBox[{
               RowBox[{"Inverse", "[", 
                RowBox[{"RigidOrientation", "[", 
                 RowBox[{"ForwardKinematics", "[", 
                  RowBox[{
                   RowBox[{"{", 
                    RowBox[{
                    RowBox[{"PrismaticTwist", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], ",", 
                    RowBox[{
                    SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
                   RowBox[{"{", 
                    RowBox[{
                    RowBox[{"PrismaticTwist", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], ",", 
                    RowBox[{
                    SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
                   RowBox[{"{", 
                    RowBox[{
                    RowBox[{"RevoluteTwist", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", 
                    RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], ",", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], 
                   ",", 
                   RowBox[{"RPToHomogeneous", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"1", ",", "0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "1", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], 
                  "]"}], "]"}], "]"}], ".", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{
                  SuperscriptBox["RigidOrientation", "\[Prime]",
                   MultilineFunction->None], "[", 
                  RowBox[{"ForwardKinematics", "[", 
                   RowBox[{
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"PrismaticTwist", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], ",", 
                    RowBox[{
                    SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"PrismaticTwist", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], ",", 
                    RowBox[{
                    SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"RevoluteTwist", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", 
                    RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], ",", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "0"], "[", "t", "]"}]}], "}"}], 
                    ",", 
                    RowBox[{"RPToHomogeneous", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"1", ",", "0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "1", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], 
                   "]"}], "]"}], " ", 
                 RowBox[{
                  SuperscriptBox["ForwardKinematics", 
                   TagBox[
                    RowBox[{"(", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"0", ",", "1"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0"}], "}"}], ",", 
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0"}], "}"}], ",", "0"}], ")"}],
                    Derivative],
                   MultilineFunction->None], "[", 
                  RowBox[{
                   RowBox[{"{", 
                    RowBox[{
                    RowBox[{"PrismaticTwist", "[", 
                    RowBox[{
                    RowBox[{"{", 
                    RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
                    TemplateBox[{"1"},
                    "OutputSizeLimit`Skeleton",
                    DisplayFunction->(FrameBox[
                    
                    RowBox[{
                    "\" \[CenterEllipsis]\"", #, "\"\[CenterEllipsis] \""}], 
                    Background -> GrayLevel[0.75], 
                    BaseStyle -> {
                    "Deploy", FontColor -> GrayLevel[1], FontSize -> Smaller, 
                    ShowStringCharacters -> False}, BaselinePosition -> 
                    Baseline, ContentPadding -> False, FrameMargins -> 1, 
                    FrameStyle -> GrayLevel[0.75], RoundingRadius -> 7]& )]}],
                     "]"}], ",", 
                    TemplateBox[{"1"},
                    "OutputSizeLimit`Skeleton",
                    DisplayFunction->(FrameBox[
                    
                    RowBox[{
                    "\" \[CenterEllipsis]\"", #, "\"\[CenterEllipsis] \""}], 
                    Background -> GrayLevel[0.75], 
                    BaseStyle -> {
                    "Deploy", FontColor -> GrayLevel[1], FontSize -> Smaller, 
                    ShowStringCharacters -> False}, BaselinePosition -> 
                    Baseline, ContentPadding -> False, FrameMargins -> 1, 
                    FrameStyle -> GrayLevel[0.75], RoundingRadius -> 7]& )]}],
                     "}"}], ",", 
                   TemplateBox[{"2"},
                    "OutputSizeLimit`Skeleton",
                    DisplayFunction->(FrameBox[
                    
                    RowBox[{
                    "\" \[CenterEllipsis]\"", #, "\"\[CenterEllipsis] \""}], 
                    Background -> GrayLevel[0.75], 
                    BaseStyle -> {
                    "Deploy", FontColor -> GrayLevel[1], FontSize -> Smaller, 
                    ShowStringCharacters -> False}, BaselinePosition -> 
                    Baseline, ContentPadding -> False, FrameMargins -> 1, 
                    FrameStyle -> GrayLevel[0.75], RoundingRadius -> 7]& )], 
                   ",", 
                   TemplateBox[{"1"},
                    "OutputSizeLimit`Skeleton",
                    DisplayFunction->(FrameBox[
                    
                    RowBox[{
                    "\" \[CenterEllipsis]\"", #, "\"\[CenterEllipsis] \""}], 
                    Background -> GrayLevel[0.75], 
                    BaseStyle -> {
                    "Deploy", FontColor -> GrayLevel[1], FontSize -> Smaller, 
                    ShowStringCharacters -> False}, BaselinePosition -> 
                    Baseline, ContentPadding -> False, FrameMargins -> 1, 
                    FrameStyle -> GrayLevel[0.75], RoundingRadius -> 7]& )]}],
                   "]"}]}], ")"}]}], " ", 
              TemplateBox[{"1"},
               "OutputSizeLimit`Skeleton",
               DisplayFunction->(FrameBox[
                 
                 RowBox[{
                  "\" \[CenterEllipsis]\"", #, "\"\[CenterEllipsis] \""}], 
                 Background -> GrayLevel[0.75], 
                 BaseStyle -> {
                  "Deploy", FontColor -> GrayLevel[1], FontSize -> Smaller, 
                   ShowStringCharacters -> False}, BaselinePosition -> 
                 Baseline, ContentPadding -> False, FrameMargins -> 1, 
                 FrameStyle -> GrayLevel[0.75], RoundingRadius -> 7]& )]}], 
             ",", 
             TemplateBox[{"1"},
              "OutputSizeLimit`Skeleton",
              DisplayFunction->(FrameBox[
                
                RowBox[{
                 "\" \[CenterEllipsis]\"", #, "\"\[CenterEllipsis] \""}], 
                Background -> GrayLevel[0.75], 
                BaseStyle -> {
                 "Deploy", FontColor -> GrayLevel[1], FontSize -> Smaller, 
                  ShowStringCharacters -> False}, BaselinePosition -> 
                Baseline, ContentPadding -> False, FrameMargins -> 1, 
                FrameStyle -> GrayLevel[0.75], RoundingRadius -> 7]& )], ",", 
             
             TemplateBox[{"5"},
              "OutputSizeLimit`Skeleton",
              DisplayFunction->(FrameBox[
                
                RowBox[{
                 "\" \[CenterEllipsis]\"", #, "\"\[CenterEllipsis] \""}], 
                Background -> GrayLevel[0.75], 
                BaseStyle -> {
                 "Deploy", FontColor -> GrayLevel[1], FontSize -> Smaller, 
                  ShowStringCharacters -> False}, BaselinePosition -> 
                Baseline, ContentPadding -> False, FrameMargins -> 1, 
                FrameStyle -> GrayLevel[0.75], RoundingRadius -> 7]& )], ",", 
             "0", ",", "0"}], "}"}], ",", 
           TemplateBox[{"1"},
            "OutputSizeLimit`Skeleton",
            DisplayFunction->(FrameBox[
              RowBox[{"\" \[CenterEllipsis]\"", #, "\"\[CenterEllipsis] \""}],
               Background -> GrayLevel[0.75], 
              BaseStyle -> {
               "Deploy", FontColor -> GrayLevel[1], FontSize -> Smaller, 
                ShowStringCharacters -> False}, BaselinePosition -> Baseline, 
              ContentPadding -> False, FrameMargins -> 1, FrameStyle -> 
              GrayLevel[0.75], RoundingRadius -> 7]& )]}], "}"}],
         Short[#, 5]& ],
        BaseStyle->{Deployed -> False},
        StripOnInput->False]},
      {GridBox[{
         {
          TagBox[
           TooltipBox[
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource[
                "FEStrings", "sizeBriefExplanation"], StandardForm],
               ImageSizeCache->{65., {3., 9.}}],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarCategory",
             StripOnInput->False],
            StyleBox[
             DynamicBox[
              ToBoxes[
               FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"], 
               StandardForm]], DynamicUpdating -> True, StripOnInput -> 
             False]],
           Annotation[#, 
            Style[
             Dynamic[
              FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"]], 
             DynamicUpdating -> True], "Tooltip"]& ], 
          ButtonBox[
           PaneSelectorBox[{False->
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], 
                StandardForm],
               ImageSizeCache->{52., {1., 9.}}],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarControl",
             StripOnInput->False], True->
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], 
                StandardForm]],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarControlActive",
             StripOnInput->False]}, Dynamic[
             CurrentValue["MouseOver"]],
            Alignment->Center,
            FrameMargins->0,
            ImageSize->{Automatic, 25}],
           Appearance->None,
           
           ButtonFunction:>OutputSizeLimit`ButtonFunction[
            Identity, 407, 22970226156311431787, 5/2],
           Enabled->True,
           Evaluator->Automatic,
           Method->"Queued"], 
          ButtonBox[
           PaneSelectorBox[{False->
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], 
                StandardForm],
               ImageSizeCache->{62., {1., 9.}}],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarControl",
             StripOnInput->False], True->
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], 
                StandardForm]],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarControlActive",
             StripOnInput->False]}, Dynamic[
             CurrentValue["MouseOver"]],
            Alignment->Center,
            FrameMargins->0,
            ImageSize->{Automatic, 25}],
           Appearance->None,
           
           ButtonFunction:>OutputSizeLimit`ButtonFunction[
            Identity, 407, 22970226156311431787, 5 2],
           Enabled->True,
           Evaluator->Automatic,
           Method->"Queued"], 
          ButtonBox[
           PaneSelectorBox[{False->
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], 
                StandardForm],
               ImageSizeCache->{44., {1., 9.}}],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarControl",
             StripOnInput->False], True->
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], 
                StandardForm]],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarControlActive",
             StripOnInput->False]}, Dynamic[
             CurrentValue["MouseOver"]],
            Alignment->Center,
            FrameMargins->0,
            ImageSize->{Automatic, 25}],
           Appearance->None,
           
           ButtonFunction:>OutputSizeLimit`ButtonFunction[
            Identity, 407, 22970226156311431787, Infinity],
           Enabled->True,
           Evaluator->Automatic,
           Method->"Queued"], 
          ButtonBox[
           PaneSelectorBox[{False->
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], 
                StandardForm],
               ImageSizeCache->{78., {1., 9.}}],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarControl",
             StripOnInput->False], True->
            StyleBox[
             StyleBox[
              DynamicBox[ToBoxes[
                FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], 
                StandardForm]],
              StripOnInput->False,
              DynamicUpdating->True], "SuggestionsBarControlActive",
             StripOnInput->False]}, Dynamic[
             CurrentValue["MouseOver"]],
            Alignment->Center,
            FrameMargins->0,
            ImageSize->{Automatic, 25}],
           Appearance->None,
           ButtonFunction:>FrontEndExecute[{
              FrontEnd`SetOptions[
              FrontEnd`$FrontEnd, 
               FrontEnd`PreferencesSettings -> {"Page" -> "Evaluation"}], 
              FrontEnd`FrontEndToken["PreferencesDialog"]}],
           Evaluator->None,
           Method->"Preemptive"]}
        },
        AutoDelete->False,
        FrameStyle->GrayLevel[0.85],
        GridBoxDividers->{"Columns" -> {False, {True}}},
        GridBoxItemSize->{"Columns" -> {{Automatic}}, "Rows" -> {{Automatic}}},
        GridBoxSpacings->{"Columns" -> {{2}}}]}
     },
     DefaultBaseStyle->"Column",
     GridBoxAlignment->{
      "Columns" -> {{Left}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
       "RowsIndexed" -> {}},
     GridBoxDividers->{
      "Columns" -> {{False}}, "ColumnsIndexed" -> {}, "Rows" -> {{False}}, 
       "RowsIndexed" -> {}},
     GridBoxItemSize->{
      "Columns" -> {{Automatic}}, "ColumnsIndexed" -> {}, "Rows" -> {{1.}}, 
       "RowsIndexed" -> {}},
     GridBoxSpacings->{"Columns" -> {
         Offset[0.27999999999999997`], {
          Offset[0.5599999999999999]}, 
         Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
         Offset[0.2], 
         Offset[1.2], {
          Offset[0.4]}, 
         Offset[0.2]}, "RowsIndexed" -> {}}],
    Background->RGBColor[0.9657, 0.9753, 0.9802],
    FrameMargins->{{12, 12}, {0, 15}},
    FrameStyle->GrayLevel[0.85],
    RoundingRadius->5,
    StripOnInput->False],
   Deploy,
   DefaultBaseStyle->"Deploy"],
  Out[407]]], "Output",
 CellChangeTimes->{{3.543080226621298*^9, 3.543080251708088*^9}, {
   3.543080295706376*^9, 3.5430803081098166`*^9}, 3.5431042401784573`*^9, 
   3.5431059789019065`*^9, {3.543106224802971*^9, 3.5431062521655364`*^9}, 
   3.543319479598028*^9, 3.543319979598627*^9, 3.543363879178399*^9, 
   3.543861905079629*^9, 3.544712504775618*^9, 3.5452658880706873`*^9, 
   3.5452666755521564`*^9, 3.545769057218392*^9, 3.547221442285622*^9, 
   3.547228098639344*^9, 3.547228514358122*^9, 3.547229425252222*^9, 
   3.547229881802335*^9, 3.547229981001009*^9, {3.547230823062172*^9, 
   3.5472308389110785`*^9}, 3.5472322553820963`*^9, 3.547240716371662*^9, 
   3.547242272667637*^9, 3.547309400579549*^9, 3.5474027905518723`*^9, 
   3.5474032558994884`*^9, 3.5474035280310535`*^9, 3.5474915524667635`*^9, 
   3.5477686302171435`*^9, 3.547830455382901*^9, 3.5478998598946123`*^9, 
   3.5515414602100124`*^9, 3.5526506379515877`*^9, 3.5526529836097517`*^9, 
   3.552828542932479*^9, 3.552932599835193*^9, 3.5563180286526346`*^9, 
   3.556325522254714*^9, 3.5563273340875907`*^9, 3.556385642334968*^9, 
   3.556833699473371*^9, 3.58133741264477*^9, 3.5880207754456005`*^9, 
   3.5926615922212396`*^9, 3.6192631235441337`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["J", "d"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"Insert", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Join", "[", 
          RowBox[{
           SubscriptBox["pe", "sf"], ",", 
           SubscriptBox["pe", "nsf"]}], "]"}]}], ",", 
        RowBox[{
         SubscriptBox["J", "\[Omega]"], "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"2", ";;", "2"}], ",", 
          RowBox[{"1", ";;", "All"}]}], "\[RightDoubleBracket]"}]}], "]"}], 
      ",", 
      RowBox[{
       SubscriptBox["J", "\[Omega]"], "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", 
        RowBox[{"1", ";;", "All"}]}], "\[RightDoubleBracket]"}], ",", "3"}], 
     "]"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.541874182918536*^9, 3.5418741891585474`*^9}, 
   3.542472550964918*^9, {3.542569192577998*^9, 3.542569195396159*^9}, {
   3.542732627480256*^9, 3.5427326463874893`*^9}, {3.5429005266538377`*^9, 
   3.5429005299922433`*^9}, {3.543079519334591*^9, 3.5430795262751513`*^9}, {
   3.5430796052513065`*^9, 3.543079625094038*^9}, {3.5431062016026444`*^9, 
   3.54310624330503*^9}, {3.547228934038126*^9, 3.547228937412319*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\({1, 3}\\)\[NoBreak] of \
\[NoBreak]\\!\\(RigidPosition[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631235851583`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "partw"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Part \[NoBreak]\\!\\({1, 3}\\)\[NoBreak] of \
\[NoBreak]\\!\\(RigidPosition[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_1[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_2[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_3[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_4[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_5[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, 1, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_6[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\[NoBreak] does not exist. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/partw\\\", ButtonNote -> \
\\\"Part::partw\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631236171775`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \
\[NoBreak]\\!\\(RigidPosition[\\(\\(ForwardKinematics[\\(\\(\\(\\({\\(\\(\
PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({1, 0, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_x[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(PrismaticTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, 0, \
1}\\)\\)\\)\\)]\\)\\), \\(\\(p\\_z[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 0}\\)\\), \\(\\({0, \\(\\(-1\\)\
\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_0[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_1[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_2[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
\\(\\(-1\\)\\), 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_3[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 2159\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_4[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 1143\\/2500}\\)\\), \\(\\({0, \
1, 0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_5[t]\\)\\)}\\)\\), \
\\(\\({\\(\\(RevoluteTwist[\\(\\(\\(\\({0, 0, 127\\/2500}\\)\\), \\(\\({0, 1, \
0}\\)\\)\\)\\)]\\)\\), \\(\\(\[Theta]\\_6[t]\\)\\)}\\)\\), \
\\(\\(RPToHomogeneous[\\(\\(\\(\\({\\(\\({1, 0, 0}\\)\\), \\(\\({0, 1, \
0}\\)\\), \\(\\({0, 0, 1}\\)\\)}\\)\\), \\(\\({0, 0, 0}\\)\\)\\)\\)]\\)\\)\\)\
\\)]\\)\\)]\\)\[NoBreak] cannot be used as a part specification. \
\\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", \
ButtonFrame->None, ButtonData:>\\\"paclet:ref/Part\\\", ButtonNote -> \
\\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123663205*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\({#2, #3}\\)\[NoBreak] \
cannot be used as a part specification. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/Part\\\", ButtonNote -> \
\\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123708232*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\({#2, #3}\\)\[NoBreak] \
cannot be used as a part specification. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/Part\\\", ButtonNote -> \
\\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123753259*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \[NoBreak]\\!\\(\\*StyleBox[\\(Part :: \
pkspec1\\), \\\"MessageName\\\"]\\)\[NoBreak] will be suppressed during this \
calculation. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/stop\\\", ButtonNote -> \
\\\"General::stop\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123769269*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Dimensions", "[", 
  SubscriptBox["J", "d"], "]"}]], "Input",
 CellChangeTimes->{{3.5418116194580464`*^9, 3.541811629150601*^9}, 
   3.541811736505741*^9, 3.5429005341418505`*^9}],

Cell[BoxData[
 RowBox[{"{", "11", "}"}]], "Output",
 CellChangeTimes->{{3.5429005345942516`*^9, 3.542900542737466*^9}, 
   3.5429021420520964`*^9, 3.5429046281730433`*^9, 3.5429070836351147`*^9, 
   3.542931217376326*^9, 3.5429313479887967`*^9, 3.542984460629484*^9, 
   3.542985315795397*^9, 3.542987469231566*^9, 3.542987508252798*^9, 
   3.5429981124493237`*^9, 3.5430000045674043`*^9, 3.543010412386592*^9, 
   3.54301072146427*^9, 3.543010905197779*^9, 3.543079451586789*^9, {
   3.543079628078209*^9, 3.543079653475279*^9}, {3.543080301804164*^9, 
   3.543080308156617*^9}, 3.5431042414015274`*^9, 3.5431059838431892`*^9, 
   3.5431062522055387`*^9, 3.5433194796280303`*^9, 3.5433199796166277`*^9, 
   3.543363879807828*^9, 3.5438619066917214`*^9, 3.54471250760878*^9, 
   3.545265890804805*^9, 3.545266676080187*^9, 3.5457690578524284`*^9, 
   3.547221442882656*^9, 3.547228101858528*^9, 3.547228516022217*^9, 
   3.547229425815254*^9, 3.5472298824553723`*^9, 3.5472299810830135`*^9, {
   3.5472308230971737`*^9, 3.5472308389660816`*^9}, 3.5472322559701295`*^9, 
   3.547242277709919*^9, 3.547309401108579*^9, 3.5474027911489067`*^9, 
   3.5474032559354906`*^9, 3.5474035280980577`*^9, 3.547491552982793*^9, 
   3.547768631476216*^9, 3.5478304558929305`*^9, 3.5478998603906407`*^9, 
   3.5515414607248135`*^9, 3.552650638431616*^9, 3.5526529836467543`*^9, 
   3.552828543430507*^9, 3.5529326003412223`*^9, 3.5563180301867228`*^9, 
   3.556325523607792*^9, 3.5563273341565943`*^9, 3.5563856439240575`*^9, 
   3.556833700367899*^9, 3.5813374128157797`*^9, 3.58802077560961*^9, 
   3.5926615927682705`*^9, 3.619263123881336*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox[
    OverscriptBox["J", "."], "d"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", "t"], 
     SubscriptBox["J", "d"]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5376215393460083`*^9, 3.537621542660198*^9}, {
   3.537621644001994*^9, 3.5376217036514063`*^9}, {3.5376266119161425`*^9, 
   3.5376266152383327`*^9}, {3.5380788445686197`*^9, 3.538078870226087*^9}, {
   3.538080112550144*^9, 3.5380801200895753`*^9}, {3.538139680143475*^9, 
   3.53813972149384*^9}, {3.538318708845624*^9, 3.538318709844026*^9}, {
   3.538318813397008*^9, 3.5383188154718113`*^9}, {3.539113543833227*^9, 
   3.5391135592460537`*^9}, {3.539113598558123*^9, 3.539113629789378*^9}, 
   3.539273191710293*^9, 3.5402484955374537`*^9, {3.5427326503654957`*^9, 
   3.5427326516758986`*^9}}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\({#2, #3}\\)\[NoBreak] \
cannot be used as a part specification. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/Part\\\", ButtonNote -> \
\\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631239553804`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\({#2, #3}\\)\[NoBreak] \
cannot be used as a part specification. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/Part\\\", ButtonNote -> \
\\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.619263123998406*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"Part", "::", "pkspec1"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"The expression \[NoBreak]\\!\\({#2, #3}\\)\[NoBreak] \
cannot be used as a part specification. \\!\\(\\*ButtonBox[\\\"\
\[RightSkeleton]\\\", ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/Part\\\", ButtonNote -> \
\\\"Part::pkspec1\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631240674477`*^9}],

Cell[BoxData[
 RowBox[{
  StyleBox[
   RowBox[{"General", "::", "stop"}], "MessageName"], 
  RowBox[{
  ":", " "}], "\<\"Further output of \[NoBreak]\\!\\(\\*StyleBox[\\(Part :: \
pkspec1\\), \\\"MessageName\\\"]\\)\[NoBreak] will be suppressed during this \
calculation. \\!\\(\\*ButtonBox[\\\"\[RightSkeleton]\\\", \
ButtonStyle->\\\"Link\\\", ButtonFrame->None, \
ButtonData:>\\\"paclet:ref/message/General/stop\\\", ButtonNote -> \
\\\"General::stop\\\"]\\)\"\>"}]], "Message", "MSG",
 CellChangeTimes->{3.6192631240694485`*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate the guard", "Subsubsection",
 CellChangeTimes->{
  3.50439250607954*^9, 3.504539201882091*^9, {3.513013067545057*^9, 
   3.513013077660357*^9}, {3.513015048236977*^9, 3.513015054334556*^9}, {
   3.513531976155762*^9, 3.513531976917461*^9}, {3.541812745932477*^9, 
   3.541812746836529*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"h", "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
     "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"hdot", " ", "=", "  ", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.516708318191987*^9, 3.516708318664014*^9}, {
   3.516754669099207*^9, 3.516754685899168*^9}, {3.541812761285355*^9, 
   3.541812790268013*^9}, 3.541813254639573*^9, {3.542569228242038*^9, 
   3.5425692283930464`*^9}, {3.54273265813431*^9, 3.542732671394333*^9}, 
   3.5429043999702415`*^9, {3.5429044707707667`*^9, 3.5429045098000355`*^9}, {
   3.5429045675669374`*^9, 3.5429045814197617`*^9}, {3.5429047835805426`*^9, 
   3.5429048625010815`*^9}, {3.5429070243706107`*^9, 3.542907077426304*^9}, {
   3.543106015033973*^9, 3.543106020138265*^9}, {3.5472289498680315`*^9, 
   3.547228950524069*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"pos", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
  "statesubs"}]], "Input"],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"0", ",", 
     RowBox[{"-", 
      FractionBox["127", "2500"]}], ",", 
     FractionBox["381", "2500"], ",", "0", ",", 
     RowBox[{
      RowBox[{"-", 
       FractionBox["254", "625"]}], " ", 
      RowBox[{"Sin", "[", 
       RowBox[{"x", "[", "1", "]"}], "]"}]}], ",", 
     RowBox[{"-", 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"320", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
          RowBox[{"321", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{"x", "[", "1", "]"}], "+", 
             RowBox[{"x", "[", "2", "]"}]}], "]"}]}]}], ")"}]}], "100000"]}], 
     ",", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"-", "40640"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{"x", "[", "1", "]"}], "]"}]}], "-", 
       RowBox[{"40767", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "-", 
       RowBox[{"34640", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}], "+", 
          RowBox[{"x", "[", "3", "]"}]}], "]"}]}]}], "100000"], ",", 
     RowBox[{"-", 
      FractionBox[
       RowBox[{"127", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{"320", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
          RowBox[{"321", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{"x", "[", "1", "]"}], "+", 
             RowBox[{"x", "[", "2", "]"}]}], "]"}]}]}], ")"}]}], "100000"]}], 
     ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"-", "320"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{"x", "[", "1", "]"}], "]"}]}], "-", 
         RowBox[{"321", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{"x", "[", "1", "]"}], "+", 
              RowBox[{"x", "[", "2", "]"}]}], "]"}], "-", 
            RowBox[{"Sin", "[", 
             RowBox[{
              RowBox[{"x", "[", "1", "]"}], "+", 
              RowBox[{"x", "[", "2", "]"}], "+", 
              RowBox[{"x", "[", "3", "]"}], "-", 
              RowBox[{"x", "[", "4", "]"}]}], "]"}]}], ")"}]}]}], ")"}]}], 
      "100000"], ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"-", "320"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{"x", "[", "1", "]"}], "]"}]}], "-", 
         RowBox[{"321", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"x", "[", "1", "]"}], "+", 
            RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "+", 
         RowBox[{"321", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"x", "[", "1", "]"}], "+", 
            RowBox[{"x", "[", "2", "]"}], "+", 
            RowBox[{"x", "[", "3", "]"}], "-", 
            RowBox[{"x", "[", "4", "]"}]}], "]"}]}], "+", 
         RowBox[{"320", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"x", "[", "1", "]"}], "+", 
            RowBox[{"x", "[", "2", "]"}], "+", 
            RowBox[{"x", "[", "3", "]"}], "-", 
            RowBox[{"x", "[", "4", "]"}], "-", 
            RowBox[{"x", "[", "5", "]"}]}], "]"}]}]}], ")"}]}], "100000"], 
     ",", 
     RowBox[{
      FractionBox["1", "100000"], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"15240", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}], "-", 
           RowBox[{"x", "[", "6", "]"}]}], "]"}]}], "-", 
        RowBox[{"40640", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{"x", "[", "1", "]"}], "]"}]}], "-", 
        RowBox[{"40767", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "+", 
        RowBox[{"40767", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}]}], "]"}]}], "+", 
        RowBox[{"40640", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}]}], "]"}]}], "+", 
        RowBox[{"4251", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}], "-", 
           RowBox[{"x", "[", "6", "]"}]}], "]"}]}]}], ")"}]}], ",", 
     RowBox[{
      FractionBox["1", "100000"], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "5080"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}], "-", 
           RowBox[{"x", "[", "6", "]"}]}], "]"}]}], "-", 
        RowBox[{"40640", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{"x", "[", "1", "]"}], "]"}]}], "-", 
        RowBox[{"40767", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "+", 
        RowBox[{"40767", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}]}], "]"}]}], "+", 
        RowBox[{"40640", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}]}], "]"}]}], "+", 
        RowBox[{"4251", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}], "-", 
           RowBox[{"x", "[", "6", "]"}]}], "]"}]}]}], ")"}]}], ",", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"-", "320"}], " ", 
          RowBox[{"Sin", "[", 
           RowBox[{"x", "[", "1", "]"}], "]"}]}], "-", 
         RowBox[{"321", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"x", "[", "1", "]"}], "+", 
            RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "+", 
         RowBox[{"321", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"x", "[", "1", "]"}], "+", 
            RowBox[{"x", "[", "2", "]"}], "+", 
            RowBox[{"x", "[", "3", "]"}], "-", 
            RowBox[{"x", "[", "4", "]"}]}], "]"}]}], "+", 
         RowBox[{"320", " ", 
          RowBox[{"Sin", "[", 
           RowBox[{
            RowBox[{"x", "[", "1", "]"}], "+", 
            RowBox[{"x", "[", "2", "]"}], "+", 
            RowBox[{"x", "[", "3", "]"}], "-", 
            RowBox[{"x", "[", "4", "]"}], "-", 
            RowBox[{"x", "[", "5", "]"}]}], "]"}]}]}], ")"}]}], "100000"]}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox["4251", "100000"], ",", "0", ",", "0", ",", 
     FractionBox["4251", "100000"], ",", 
     FractionBox[
      RowBox[{"4251", "+", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{"x", "[", "1", "]"}], "]"}]}]}], "100000"], ",", 
     FractionBox[
      RowBox[{"4251", "+", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}]}], "]"}]}]}], "100000"], ",", 
     FractionBox[
      RowBox[{"4251", "+", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "+", 
       RowBox[{"34640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}], "+", 
          RowBox[{"x", "[", "3", "]"}]}], "]"}]}]}], "100000"], ",", 
     FractionBox[
      RowBox[{"4251", "+", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}]}], "]"}]}]}], "100000"], ",", 
     FractionBox[
      RowBox[{"4251", "+", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "-", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}], "+", 
          RowBox[{"x", "[", "3", "]"}], "-", 
          RowBox[{"x", "[", "4", "]"}]}], "]"}]}]}], "100000"], ",", 
     FractionBox[
      RowBox[{"4251", "+", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "-", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}], "+", 
          RowBox[{"x", "[", "3", "]"}], "-", 
          RowBox[{"x", "[", "4", "]"}]}], "]"}]}], "-", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}], "+", 
          RowBox[{"x", "[", "3", "]"}], "-", 
          RowBox[{"x", "[", "4", "]"}], "-", 
          RowBox[{"x", "[", "5", "]"}]}], "]"}]}]}], "100000"], ",", 
     RowBox[{
      FractionBox["1", "100000"], 
      RowBox[{"(", 
       RowBox[{"4251", "+", 
        RowBox[{"40640", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
        RowBox[{"40767", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "-", 
        RowBox[{"40767", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}]}], "]"}]}], "-", 
        RowBox[{"40640", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}]}], "]"}]}], "-", 
        RowBox[{"4251", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}], "-", 
           RowBox[{"x", "[", "6", "]"}]}], "]"}]}], "+", 
        RowBox[{"15240", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}], "-", 
           RowBox[{"x", "[", "6", "]"}]}], "]"}]}]}], ")"}]}], ",", 
     RowBox[{
      FractionBox["1", "100000"], 
      RowBox[{"(", 
       RowBox[{"4251", "+", 
        RowBox[{"40640", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
        RowBox[{"40767", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "-", 
        RowBox[{"40767", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}]}], "]"}]}], "-", 
        RowBox[{"40640", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}]}], "]"}]}], "-", 
        RowBox[{"4251", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}], "-", 
           RowBox[{"x", "[", "6", "]"}]}], "]"}]}], "-", 
        RowBox[{"5080", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"x", "[", "1", "]"}], "+", 
           RowBox[{"x", "[", "2", "]"}], "+", 
           RowBox[{"x", "[", "3", "]"}], "-", 
           RowBox[{"x", "[", "4", "]"}], "-", 
           RowBox[{"x", "[", "5", "]"}], "-", 
           RowBox[{"x", "[", "6", "]"}]}], "]"}]}]}], ")"}]}], ",", 
     FractionBox[
      RowBox[{"4251", "+", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{"x", "[", "1", "]"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}]}], "]"}]}], "-", 
       RowBox[{"40767", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}], "+", 
          RowBox[{"x", "[", "3", "]"}], "-", 
          RowBox[{"x", "[", "4", "]"}]}], "]"}]}], "-", 
       RowBox[{"40640", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"x", "[", "1", "]"}], "+", 
          RowBox[{"x", "[", "2", "]"}], "+", 
          RowBox[{"x", "[", "3", "]"}], "-", 
          RowBox[{"x", "[", "4", "]"}], "-", 
          RowBox[{"x", "[", "5", "]"}]}], "]"}]}]}], "100000"]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{3.54749119084608*^9, 3.5474915548819017`*^9, 
  3.5477686362914915`*^9, 3.5478304577810383`*^9, 3.5478998622587476`*^9, 
  3.5515414626280165`*^9, 3.5526506402877216`*^9, 3.552652983800763*^9, 
  3.5528285452986145`*^9, 3.5529326022203293`*^9, 3.5563180356560354`*^9, 
  3.556325528736085*^9, 3.5563273345056143`*^9, 3.5563856495901203`*^9, 
  3.556833703744308*^9, 3.5813374247694635`*^9, 3.5880207876262975`*^9, 
  3.5926615948793917`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<jpos_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"pos", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", " ", "=", " ", 
   RowBox[{"OpenWrite", "[", "\"\<vtoe\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"vtoe", "/.", "constsubs"}], "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513876298225372*^9, 3.513876314895954*^9}, 
   3.5207161215675383`*^9, {3.54749018694266*^9, 3.547490235142417*^9}, {
   3.5474903252195687`*^9, 3.547490329650822*^9}, {3.5474913627339115`*^9, 
   3.5474913818560047`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[ScriptCapitalD]e", "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "exstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014078429472*^9}, 3.513014297088332*^9, {3.51301484393123*^9, 
   3.513014845159042*^9}, {3.513015197361516*^9, 3.513015197908276*^9}, {
   3.513887638252694*^9, 3.513887640636993*^9}, 3.5207161274308734`*^9, {
   3.5427343572421827`*^9, 3.5427343574501944`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<D_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[ScriptCapitalD]", "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014078429472*^9}, 3.513014297088332*^9, {3.513015201746145*^9, 
   3.513015202045883*^9}, 3.5207161235046487`*^9, {3.5427343543952255`*^9, 
   3.5427343761545725`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[ScriptCapitalC]", "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014076194914*^9}, 3.513014294914336*^9, {3.513015198946243*^9, 
   3.513015199252923*^9}, 3.5207161246117125`*^9, {3.5427343507460194`*^9, 
   3.5427343523461084`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Ce_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[ScriptCapitalC]e", "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "exstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5427327113616033`*^9, 3.542732720472019*^9}, {
  3.5427343791463327`*^9, 3.542734379354345*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Ge_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[ScriptCapitalG]e", "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "exstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.512953878469522*^9, 3.512953886446665*^9}, {
   3.512994085488056*^9, 3.512994090994364*^9}, 3.512994135460699*^9, {
   3.513013423442089*^9, 3.513013424564995*^9}, {3.513014073959661*^9, 
   3.513014076194914*^9}, {3.513014291511217*^9, 3.51301431273912*^9}, {
   3.513015194622436*^9, 3.513015194950777*^9}, {3.5158609614944687`*^9, 
   3.515860974401207*^9}, 3.520716126131799*^9, {3.5427343944424872`*^9, 
   3.5427343958823633`*^9}, 3.542734453258155*^9, {3.542735295240738*^9, 
   3.5427352963934016`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[ScriptCapitalG]", "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5427353024410925`*^9, 3.5427353090940533`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Ee_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["J", "d"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "exstatesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015189396898*^9, 3.513015190520248*^9}, {3.513887472707003*^9, 
   3.51388747514813*^9}, {3.513887645297176*^9, 3.513887645992246*^9}, 
   3.5207161322741504`*^9, {3.5418740297886677`*^9, 3.541874059085519*^9}, 
   3.5427343980424843`*^9, {3.5427344466026945`*^9, 3.5427344512910585`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Edot_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox[
       OverscriptBox["J", "."], "d"], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "exstatesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5429311591179943`*^9, 3.542931182725344*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"h", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015181146682*^9, 3.513015186905603*^9}, {3.513887467972675*^9, 
   3.513887469003708*^9}, {3.513887648118017*^9, 3.513887648448756*^9}, 
   3.5207161339002433`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"hdot", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.4978952481411505`*^9, 3.4978953056554403`*^9}, {
   3.504541084026764*^9, 3.504541092118987*^9}, {3.504545862009512*^9, 
   3.504545864143497*^9}, 3.504652479388712*^9, {3.5125119377519436`*^9, 
   3.512511937845544*^9}, {3.512525693637813*^9, 3.512525694152614*^9}, {
   3.5125257246194677`*^9, 3.512525725415069*^9}, {3.513015036076006*^9, 
   3.513015036343996*^9}, {3.51301512396632*^9, 3.513015127387151*^9}, {
   3.513015181146682*^9, 3.513015186905603*^9}, {3.513887467972675*^9, 
   3.513887469003708*^9}, {3.513887648118017*^9, 3.513887648448756*^9}, {
   3.5167541423880806`*^9, 3.5167541451912413`*^9}, 3.52071613524032*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Choosing the Outputs", "Section",
 CellChangeTimes->{{3.5206963594857893`*^9, 3.520696361509555*^9}, {
  3.5210630974288096`*^9, 3.521063101088019*^9}}],

Cell["calculate COM for later use", "Text",
 CellChangeTimes->{{3.527517849280295*^9, 3.527517851164325*^9}, {
  3.527518048751359*^9, 3.52751805237524*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["p", "COM"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     FractionBox[
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], "ndof"], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
         "\[RightDoubleBracket]"}], " ", 
        RowBox[{
        "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}]}]}], 
      RowBox[{"Total", "[", "mm", "]"}]], "/.", "\[InvisibleSpace]", 
     "constsubs"}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.527517851934319*^9, 3.527517873584816*^9}, {
   3.527517942241016*^9, 3.527518031085544*^9}, {3.527518364898716*^9, 
   3.527518402736513*^9}, 3.527540516409532*^9, {3.5278756837753963`*^9, 
   3.5278757363124013`*^9}, {3.5375589436087866`*^9, 3.537559002523156*^9}, 
   3.537561380470167*^9, {3.537562803416555*^9, 3.537562817710373*^9}, {
   3.538229844412299*^9, 3.538229859500162*^9}, {3.5386888606316333`*^9, 
   3.5386888707092094`*^9}, {3.5386889046441507`*^9, 
   3.5386889390671196`*^9}, {3.5387486662928343`*^9, 
   3.5387486698290367`*^9}, {3.542731005716607*^9, 3.542731008041011*^9}}],

Cell[CellGroupData[{

Cell["Torso Angle", "Subsection",
 CellChangeTimes->{{3.5206963594857893`*^9, 3.520696361509555*^9}}],

Cell[CellGroupData[{

Cell["Torso angle", "Subsubsection",
 CellChangeTimes->{{3.5206955441956043`*^9, 3.520695554257166*^9}, 
   3.5418143566776066`*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"TorsoAngle", "=", 
  RowBox[{
   UnderoverscriptBox["\[Sum]", "i", "3"], 
   RowBox[{
    SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.52069555679044*^9, 3.520695570667728*^9}, {
   3.520709652691082*^9, 3.520709653905396*^9}, {3.541813857181037*^9, 
   3.5418138978823647`*^9}, 3.5418139603999405`*^9, 3.5418143665181694`*^9, {
   3.54273101877383*^9, 3.542731024935841*^9}, {3.54273136585884*^9, 
   3.542731386060875*^9}, {3.542731419382534*^9, 3.5427314197101345`*^9}, {
   3.5429005671047087`*^9, 3.542900569756713*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
  RowBox[{
   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
  RowBox[{
   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]], "Output",
 CellChangeTimes->{
  3.542900570115514*^9, 3.542902142405116*^9, 3.542904628531844*^9, 
   3.5429070839627156`*^9, 3.5429312191664286`*^9, 3.542931348378819*^9, 
   3.542985316974464*^9, 3.542987508599818*^9, 3.5429981151204767`*^9, 
   3.543000004941805*^9, 3.54301041392668*^9, 3.5430107232523727`*^9, 
   3.543010905767812*^9, 3.543079454581994*^9, 3.543079653954306*^9, 
   3.5430803140790496`*^9, 3.543106032198955*^9, 3.5431062574968414`*^9, 
   3.543319480100057*^9, 3.5433199800346518`*^9, 3.5433638820599566`*^9, 
   3.543861911174978*^9, 3.5452658953010573`*^9, 3.5452666782273097`*^9, 
   3.5457690601335588`*^9, 3.547221445661815*^9, 3.5472285494221272`*^9, 
   3.5472294290874414`*^9, 3.5472298859005694`*^9, 3.5472299816770477`*^9, {
   3.5472308236482058`*^9, 3.547230839482111*^9}, 3.5472322590013027`*^9, 
   3.547242304558171*^9, 3.547309403585721*^9, 3.547402793752055*^9, 
   3.5474032563155127`*^9, 3.5474035285080805`*^9, 3.5474915553399277`*^9, 
   3.5477686370745363`*^9, 3.5478304582420645`*^9, 3.547899862708773*^9, 
   3.5515414630804176`*^9, 3.5526506407167463`*^9, 3.5526529841237817`*^9, 
   3.552828545804643*^9, 3.5529326027413597`*^9, 3.556318036809101*^9, 
   3.5563255296091347`*^9, 3.5563273349826417`*^9, 3.5563856504701223`*^9, 
   3.5568337046972933`*^9, 3.58133742785364*^9, 3.588020791094496*^9, 
   3.592661595469425*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Torso Hip angle", "Subsubsection",
 CellChangeTimes->{3.520695578765766*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"TorsoHipAngle", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.5206945398251047`*^9, 3.520694570527237*^9}, {
   3.5206954481403933`*^9, 3.520695451085517*^9}, {3.520709696720999*^9, 
   3.520709699861195*^9}, {3.520710054090211*^9, 3.520710059923527*^9}, 
   3.5418143685822873`*^9, {3.5427310294130487`*^9, 3.542731040286268*^9}, {
   3.5427310834359436`*^9, 3.542731124698016*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]], "Output",
 CellChangeTimes->{
  3.5427311252284174`*^9, 3.5427317938667917`*^9, 3.542733698160447*^9, 
   3.5427344124218435`*^9, 3.542734459865829*^9, 3.5427345872660894`*^9, 
   3.542734729730768*^9, 3.542734864085552*^9, 3.5427354231664705`*^9, 
   3.5427361536145077`*^9, 3.542736258133008*^9, 3.542736794088066*^9, 
   3.5427368592326155`*^9, 3.542900687335592*^9, 3.542902142435118*^9, 
   3.5429046285474443`*^9, 3.5429070839783154`*^9, 3.542931219179429*^9, 
   3.5429313484048204`*^9, 3.5429853169894648`*^9, 3.542987508616819*^9, 
   3.542998115137478*^9, 3.543000004957405*^9, 3.543010413946681*^9, 
   3.5430107232743735`*^9, 3.543010905789813*^9, 3.5430794545975943`*^9, 
   3.543079653982308*^9, 3.543080314104051*^9, 3.5431060322149563`*^9, 
   3.543106257512842*^9, 3.5433194801070576`*^9, 3.543319980039652*^9, 
   3.5433638820669565`*^9, 3.5438619112069798`*^9, 3.5452658953340592`*^9, 
   3.5452666782323103`*^9, 3.545769060136559*^9, 3.547221445675816*^9, 
   3.54722854946913*^9, 3.547229429122443*^9, 3.547229885936571*^9, 
   3.5472299817310505`*^9, {3.547230823672207*^9, 3.547230839506112*^9}, 
   3.5472322590153036`*^9, 3.5472423046481705`*^9, 3.5473094036127224`*^9, 
   3.5474027937950573`*^9, 3.547403256335513*^9, 3.5474035285560837`*^9, 
   3.5474915553879304`*^9, 3.5477686370945373`*^9, 3.547830458279067*^9, 
   3.5478998627367744`*^9, 3.5515414631116176`*^9, 3.5526506407377477`*^9, 
   3.552652984147783*^9, 3.552828545831645*^9, 3.5529326027553606`*^9, 
   3.556318036870105*^9, 3.5563255296681385`*^9, 3.5563273350266447`*^9, 
   3.556385650540122*^9, 3.556833704737903*^9, 3.5813374278756413`*^9, 
   3.588020791115497*^9, 3.592661595488426*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Torso non-stance thigh angle", "Subsubsection",
 CellChangeTimes->{{3.5206955441956043`*^9, 3.520695554257166*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"outputsTorso", " ", "=", " ", "TorsoAngle"}], ";"}]], "Input",
 CellChangeTimes->{{3.5418146217817698`*^9, 3.5418146328814044`*^9}, {
   3.541870851866314*^9, 3.541870859182727*^9}, {3.5425504111854196`*^9, 
   3.5425504129455204`*^9}, 3.5425505156143923`*^9, 3.54255662260425*^9, {
   3.542557113699977*^9, 3.542557125050993*^9}, {3.5427315266463227`*^9, 
   3.542731529189127*^9}}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Non-Stance Leg", "Subsection",
 CellChangeTimes->{{3.520696369485465*^9, 3.520696373189542*^9}, {
  3.527518724466259*^9, 3.527518730429883*^9}, {3.52754059614551*^9, 
  3.527540598788119*^9}}],

Cell[CellGroupData[{

Cell["non-stance slope", "Subsubsection",
 CellChangeTimes->{{3.520696408621931*^9, 3.5206964120770273`*^9}, 
   3.5384851650680585`*^9, 3.538689856414589*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"outputsNSLeg", "=", 
   RowBox[{"{", 
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{"(", 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsa"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "ship"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsa"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "ship"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], ")"}], "]"}], 
    "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.527518748452339*^9, 3.527518764917305*^9}, {
  3.5375590640716767`*^9, 3.5375590701520243`*^9}, {3.53868926794293*^9, 
  3.538689269045993*^9}, {3.538758158260744*^9, 3.5387581591397943`*^9}, {
  3.5387583015289383`*^9, 3.5387583203020124`*^9}, {3.5387584521335526`*^9, 
  3.538758453509631*^9}, {3.541814152112906*^9, 3.541814191663168*^9}, {
  3.5418152514167824`*^9, 3.541815263806491*^9}, {3.541815306325923*^9, 
  3.541815313133312*^9}, {3.547228969726167*^9, 3.547228977445609*^9}, {
  3.547229658192545*^9, 3.5472296640648813`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["g", "nsa"], "[", "\[Theta]", "]"}], "\[LeftDoubleBracket]", 
  RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]], "Input",
 CellChangeTimes->{{3.547229148913416*^9, 3.5472291495534525`*^9}}],

Cell[BoxData[
 FractionBox[
  RowBox[{"127", " ", 
   RowBox[{"(", 
    RowBox[{
     RowBox[{
      RowBox[{"-", "320"}], " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
     RowBox[{"321", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
     RowBox[{"321", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
     RowBox[{"320", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], ")"}]}],
   "100000"]], "Output",
 CellChangeTimes->{
  3.5427361538395205`*^9, {3.542736258423024*^9, 3.5427362768668346`*^9}, 
   3.5427367943570814`*^9, 3.5427368595066276`*^9, 3.5429006432343144`*^9, 
   3.5429021426321297`*^9, 3.5429046287034445`*^9, 3.542907084134316*^9, 
   3.54293121937444*^9, 3.542931348606832*^9, 3.54298531708047*^9, 
   3.542987508709824*^9, 3.542998115249484*^9, 3.543000005051005*^9, 
   3.543010414044687*^9, 3.5430107233843803`*^9, 3.5430109058978195`*^9, 
   3.543079454737994*^9, 3.543079654102315*^9, 3.543080314221058*^9, 
   3.543106032306961*^9, 3.543106257601847*^9, 3.5433194801900625`*^9, 
   3.5433199801196566`*^9, 3.5433638821499615`*^9, 3.5438619113479877`*^9, 
   3.545265895479068*^9, 3.545266678310314*^9, 3.5457690602095633`*^9, 
   3.5472214457658215`*^9, 3.5472285495831366`*^9, 3.5472294294434614`*^9, 
   3.5472298860465775`*^9, 3.547229981863058*^9, {3.5472308237682123`*^9, 
   3.5472308396041183`*^9}, 3.547232259140311*^9, 3.5472423051631727`*^9, 
   3.5473094037147284`*^9, 3.547402793906064*^9, 3.5474032564205184`*^9, 
   3.5474035286730905`*^9, 3.547491555502937*^9, 3.547768637213544*^9, 
   3.5478304583870735`*^9, 3.5478998628367805`*^9, 3.5515414632208176`*^9, 
   3.552650640829753*^9, 3.5526529842437887`*^9, 3.5528285459306507`*^9, 
   3.5529326028423653`*^9, 3.556318037050115*^9, 3.556325529835148*^9, 
   3.5563273351686525`*^9, 3.5563856507081304`*^9, 3.5568337048970537`*^9, 
   3.581337427935645*^9, 3.5880207911745005`*^9, 3.592661595573431*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["g", "ship"], "[", "\[Theta]", "]"}], "\[LeftDoubleBracket]", 
  
  RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]], "Input",
 CellChangeTimes->{3.5472296878142395`*^9}],

Cell[BoxData[
 RowBox[{"-", 
  FractionBox[
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"320", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
      RowBox[{"321", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
     ")"}]}], "100000"]}]], "Output",
 CellChangeTimes->{
  3.5427361538685217`*^9, 3.5427362584640265`*^9, 3.542736794396084*^9, 
   3.5427368595386295`*^9, 3.542900645231118*^9, 3.542902142663131*^9, 
   3.542904628734645*^9, 3.5429070841499157`*^9, 3.5429312193894415`*^9, 
   3.5429313486318336`*^9, 3.542985317097471*^9, 3.542987508728825*^9, 
   3.5429981152724857`*^9, 3.543000005066605*^9, 3.5430104140646877`*^9, 
   3.5430107234103813`*^9, 3.54301090592082*^9, 3.5430794547691946`*^9, 
   3.543079654131316*^9, 3.5430803142510595`*^9, 3.543106032323962*^9, 
   3.5431062576208487`*^9, 3.5433194802000628`*^9, 3.543319980127657*^9, 
   3.5433638821589622`*^9, 3.54386191138399*^9, 3.5452658955140696`*^9, 
   3.545266678319315*^9, 3.545769060215564*^9, 3.547221445783822*^9, 
   3.5472285496301394`*^9, 3.5472294294534616`*^9, 3.54722988608358*^9, 
   3.5472299819180613`*^9, {3.5472308237912135`*^9, 3.54723083962912*^9}, 
   3.547232259158312*^9, 3.5472423052381725`*^9, 3.5473094037437296`*^9, 
   3.5474027939500666`*^9, 3.547403256436519*^9, 3.547403528721093*^9, 
   3.5474915555509396`*^9, 3.5477686372405453`*^9, 3.547830458423075*^9, 
   3.5478998628647823`*^9, 3.551541463252018*^9, 3.552650640850754*^9, 
   3.55265298427179*^9, 3.552828545958652*^9, 3.552932602858366*^9, 
   3.5563180371141186`*^9, 3.556325529897151*^9, 3.556327335221656*^9, 
   3.5563856507571335`*^9, 3.556833704928035*^9, 3.5813374279606466`*^9, 
   3.5880207911985016`*^9, 3.592661595582432*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["linearized non-stance slope", "Subsubsection",
 CellChangeTimes->{{3.520696416053858*^9, 3.5206964219347477`*^9}, 
   3.5384851609652514`*^9, 3.538689851174289*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"outputsNSLeg", "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{"Linearize", "[", 
      RowBox[{"#", ",", 
       RowBox[{"Flatten", "[", "qe", "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", "9", "}"}]}], "]"}]}], "]"}], "&"}], "/@", 
    RowBox[{
    "outputsNSLeg", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], 
   "//", "ParallelSimplify"}]}]], "Input",
 CellChangeTimes->{{3.5276174796189756`*^9, 3.5276175122438416`*^9}, 
   3.5375590203131742`*^9, {3.537559254136548*^9, 3.5375592702674704`*^9}, {
   3.5375593282407866`*^9, 3.5375593409075108`*^9}, {3.538485138048811*^9, 
   3.538485150856434*^9}, {3.538485210245738*^9, 3.5384852133501434`*^9}, {
   3.5384915583560886`*^9, 3.5384915635352983`*^9}, {3.538689286850012*^9, 
   3.5386893440982857`*^9}, {3.538758142996871*^9, 3.5387581735486183`*^9}, {
   3.5387583060411963`*^9, 3.538758325899332*^9}, {3.538758457286847*^9, 
   3.538758482106267*^9}, {3.541814183669711*^9, 3.541814185245801*^9}, 
   3.541814372461509*^9, 3.541815213456611*^9, {3.541872994362321*^9, 
   3.5418729983403277`*^9}, {3.5427358138499765`*^9, 3.542735838393467*^9}, {
   3.542988276741753*^9, 3.542988285916278*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"-", 
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "+", 
  RowBox[{
   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
  FractionBox[
   RowBox[{"320", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "641"]}]], "Output",
 CellChangeTimes->{
  3.541815322377841*^9, 3.541818718073064*^9, 3.541819460203511*^9, 
   3.541863467793748*^9, 3.541868247930873*^9, 3.541870796971817*^9, 
   3.541872999213929*^9, 3.5424714618837333`*^9, 3.5424725659097443`*^9, 
   3.5424800298087454`*^9, 3.542484081881524*^9, 3.542494457137398*^9, 
   3.5425426487882094`*^9, 3.5425430155601873`*^9, 3.542549680976654*^9, 
   3.542556630368261*^9, 3.5425571431620183`*^9, 3.5425692743106728`*^9, 
   3.542569339029374*^9, 3.542570062016727*^9, 3.542731162980483*^9, 
   3.542731794459593*^9, 3.5427336987214756`*^9, 3.5427344129708633`*^9, 
   3.542734460446862*^9, 3.5427345878391223`*^9, 3.5427347302885976`*^9, 
   3.542734864669585*^9, 3.542735423471488*^9, {3.542735814263998*^9, 
   3.5427358418814573`*^9}, 3.542736153900524*^9, 3.542736258497029*^9, 
   3.5427367944440866`*^9, 3.5427368595746317`*^9, {3.5429005957931585`*^9, 
   3.542900647009521*^9}, 3.542902142696133*^9, 3.542904628750245*^9, 
   3.542907084165516*^9, 3.542931219406442*^9, 3.542931348661835*^9, 
   3.5429853171164722`*^9, {3.542987480891233*^9, 3.542987508746826*^9}, {
   3.5429882876293755`*^9, 3.5429882943037577`*^9}, 3.5429981152954865`*^9, 
   3.5430000050822053`*^9, 3.5430104141076903`*^9, 3.543010723440383*^9, 
   3.543010905946822*^9, 3.543079454800394*^9, 3.543079654162318*^9, 
   3.543080314283061*^9, 3.543106032344963*^9, 3.5431062576418495`*^9, 
   3.5433194802140636`*^9, 3.5433199801386576`*^9, 3.5433638821719627`*^9, 
   3.543861911423992*^9, 3.5452658955520716`*^9, 3.5452666783303156`*^9, 
   3.545769060223564*^9, 3.547221445804824*^9, 3.5472285496781416`*^9, 
   3.5472294294954643`*^9, 3.54722988609558*^9, 3.5472299819730644`*^9, {
   3.5472308238162155`*^9, 3.547230839657121*^9}, 3.5472322591783133`*^9, 
   3.547242305338173*^9, 3.5473094037747316`*^9, 3.5474027939980693`*^9, 
   3.54740325645652*^9, 3.547403528771096*^9, 3.5474915556009426`*^9, 
   3.547768637270547*^9, 3.547830458462077*^9, 3.547899862895784*^9, 
   3.551541463298818*^9, 3.5526506408757553`*^9, 3.5526529843017917`*^9, 
   3.5528285459886537`*^9, 3.5529326028783674`*^9, 3.5563180371831226`*^9, 
   3.556325529966155*^9, 3.5563273352726583`*^9, 3.556385650806136*^9, 
   3.5568337049640903`*^9, 3.5813374281166553`*^9, 3.588020791395513*^9, 
   3.592661595613434*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Stance Knee", "Subsection",
 CellChangeTimes->{{3.527517127211601*^9, 3.527517129005871*^9}, 
   3.5384849013641953`*^9, {3.5386898445419097`*^9, 3.538689846342013*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"outputsSTKnee", "=", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.527517132886071*^9, 3.527517140125339*^9}, {
  3.527540548617001*^9, 3.527540554878342*^9}, {3.5384845877568445`*^9, 
  3.538484605806076*^9}, {3.5384847990748158`*^9, 3.53848480143042*^9}, {
  3.5384856414000387`*^9, 3.53848564218004*^9}, {3.538689363377389*^9, 
  3.538689381984453*^9}, {3.5387486912672625`*^9, 3.538748695215489*^9}, {
  3.541814266858469*^9, 3.5418142718817563`*^9}, {3.5427311944769387`*^9, 
  3.54273119503854*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Non-Stance Knee", "Subsection",
 CellChangeTimes->{{3.527540532146779*^9, 3.527540540301477*^9}, {
   3.53848488702777*^9, 3.538484896403387*^9}, 3.5386898813110127`*^9}],

Cell["relative angle of non-stance knee", "Text",
 CellChangeTimes->{{3.538690398220578*^9, 3.5386904024848223`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"outputsNSKnee", "=", 
   RowBox[{
    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.527540542257499*^9, 3.527540561881515*^9}, 
   3.52754225028292*^9, {3.538484665928582*^9, 3.5384846800778065`*^9}, {
   3.5384848120696383`*^9, 3.5384848142224426`*^9}, 3.5384848813337603`*^9, {
   3.5386901860554433`*^9, 3.5386901945989323`*^9}, {3.5387486986086826`*^9, 
   3.538748702018878*^9}, {3.54181427824212*^9, 3.541814284057453*^9}, {
   3.542731231261803*^9, 3.542731232603406*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Non-Stance foot roll angle", "Subsection",
 CellChangeTimes->{{3.527517127211601*^9, 3.527517129005871*^9}, 
   3.5384849013641953`*^9, {3.5386898445419097`*^9, 3.538689846342013*^9}, {
   3.5418178720986767`*^9, 3.541817878658052*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"outputsNSfoot", "=", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"1", ",", "1", ",", "1", ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}]}], "}"}], ".", 
   RowBox[{"Flatten", "[", "q", "]"}]}]}]], "Input",
 CellChangeTimes->{{3.527517132886071*^9, 3.527517140125339*^9}, {
  3.527540548617001*^9, 3.527540554878342*^9}, {3.5384845877568445`*^9, 
  3.538484605806076*^9}, {3.5384847990748158`*^9, 3.53848480143042*^9}, {
  3.5384856414000387`*^9, 3.53848564218004*^9}, {3.538689363377389*^9, 
  3.538689381984453*^9}, {3.5387486912672625`*^9, 3.538748695215489*^9}, {
  3.541814266858469*^9, 3.5418142718817563`*^9}, {3.5418178824272676`*^9, 
  3.541817986375213*^9}, {3.5427312365034122`*^9, 3.5427312445218267`*^9}, {
  3.542731432424157*^9, 3.5427314476809835`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
  RowBox[{
   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
  RowBox[{
   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]], "Output",
 CellChangeTimes->{
  3.54273146809942*^9, 3.5427317945219927`*^9, 3.542733698746477*^9, 
   3.5427344130208635`*^9, 3.5427344604908648`*^9, 3.542734587891125*^9, 
   3.5427347303015985`*^9, 3.542734864732589*^9, 3.5427354235134907`*^9, 
   3.542736153940526*^9, 3.5427362585340304`*^9, 3.54273679450209*^9, 
   3.5427368596266346`*^9, 3.5429006568843384`*^9, 3.542902142733135*^9, 
   3.542904628765845*^9, 3.542907084181116*^9, 3.542931219430444*^9, 
   3.542931348695837*^9, 3.542985317141474*^9, 3.5429875087728276`*^9, 
   3.542998115318488*^9, 3.543000005113405*^9, 3.5430104141656933`*^9, 
   3.543010723478385*^9, 3.5430109059788237`*^9, 3.5430794548627944`*^9, 
   3.5430796541993203`*^9, 3.5430803143180633`*^9, 3.543106032370965*^9, 
   3.543106257666851*^9, 3.543319480231065*^9, 3.5433199801506586`*^9, 
   3.543363882188964*^9, 3.5438619115349984`*^9, 3.545265895593074*^9, 
   3.5452666783423166`*^9, 3.545769060230564*^9, 3.5472214458318253`*^9, 
   3.547228549734145*^9, 3.5472294295364666`*^9, 3.547229886138583*^9, 
   3.547229982048069*^9, {3.5472308238452168`*^9, 3.547230839686123*^9}, 
   3.547232259199314*^9, 3.5472423056031733`*^9, 3.5473094038127337`*^9, 
   3.547402794048072*^9, 3.5474032564995227`*^9, 3.547403528827099*^9, 
   3.5474915556899476`*^9, 3.5477686373115497`*^9, 3.54783045850908*^9, 
   3.547899862932786*^9, 3.551541463330018*^9, 3.552650640910757*^9, 
   3.552652984336794*^9, 3.552828546028656*^9, 3.5529326029023685`*^9, 
   3.556318037273128*^9, 3.55632553004416*^9, 3.5563273353446627`*^9, 
   3.556385650894141*^9, 3.55683370502281*^9, 3.58133742819666*^9, 
   3.5880207914305153`*^9, 3.592661595640435*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Hip Position", "Subsection",
 CellChangeTimes->{{3.5206971864248543`*^9, 3.520697188943389*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"outputsHip", "=", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"ParallelSimplify", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "ship"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "sa"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], "]"}], "}"}], 
   "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}]], "Input",
 CellChangeTimes->{{3.527519011730059*^9, 3.527519021263712*^9}, 
   3.5386906952165656`*^9, {3.5386907415512156`*^9, 3.5386907429192944`*^9}, {
   3.538690816839522*^9, 3.538690818015589*^9}, {3.5387346181523266`*^9, 
   3.5387346755386095`*^9}, {3.5387489985828404`*^9, 
   3.5387490000699253`*^9}, {3.5387491492594585`*^9, 3.538749159163025*^9}, {
   3.538749201418442*^9, 3.5387492044356146`*^9}, {3.538749279761923*^9, 
   3.5387492829111032`*^9}, {3.5387582295448213`*^9, 
   3.5387582472488337`*^9}, {3.538758367974739*^9, 3.538758370476882*^9}, {
   3.5391997279765267`*^9, 3.539199731923334*^9}, {3.53919995184693*^9, 
   3.539199958118141*^9}, 3.539200063231126*^9, {3.5392712167247133`*^9, 
   3.539271245459964*^9}, {3.5392871277309656`*^9, 3.5392871698822393`*^9}, {
   3.5392872960157375`*^9, 3.539287297014139*^9}, {3.5399133343253245`*^9, 
   3.5399133372014894`*^9}, {3.5418142956811175`*^9, 3.5418143007134056`*^9}, 
   3.5418143752696695`*^9, {3.541814504096038*^9, 3.5418145088083076`*^9}, 
   3.5418145468864856`*^9, {3.5425694968374004`*^9, 3.5425694969344063`*^9}, {
   3.542570090539358*^9, 3.542570090675366*^9}, {3.542731479986641*^9, 
   3.542731483590247*^9}, {3.5472291698416133`*^9, 3.5472291699216175`*^9}, 
   3.5472296961187143`*^9}],

Cell[BoxData[
 RowBox[{"-", 
  FractionBox[
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"320", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
      RowBox[{"321", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
     ")"}]}], "100000"]}]], "Output",
 CellChangeTimes->{
  3.5418682479776735`*^9, 3.541870797034217*^9, 3.5418729330542135`*^9, 
   3.542471461961733*^9, 3.5424725660189447`*^9, 3.542480029929946*^9, 
   3.5424840819283247`*^9, 3.5424944571944013`*^9, 3.542542648851213*^9, 
   3.542543015619191*^9, 3.542549732621608*^9, 3.5425566304182606`*^9, 
   3.5425571432020187`*^9, 3.542569274368676*^9, 3.5425693390853777`*^9, 
   3.542569498011468*^9, 3.5425695435350714`*^9, 3.542570091529415*^9, 
   3.542731248406234*^9, {3.5427314769602356`*^9, 3.5427314997674756`*^9}, 
   3.5427317945687933`*^9, 3.5427336987744784`*^9, 3.5427344130608635`*^9, 
   3.5427344605238667`*^9, 3.5427345879311275`*^9, 3.5427347303175993`*^9, 
   3.542734864801593*^9, 3.5427354235604925`*^9, 3.542736153978528*^9, 
   3.542736258577031*^9, 3.542736794544092*^9, 3.5427368596646366`*^9, 
   3.542900661283546*^9, 3.5429021427721376`*^9, 3.5429046287970448`*^9, 
   3.5429070841967163`*^9, 3.542931219454445*^9, 3.542931348728839*^9, 
   3.5429853171644745`*^9, 3.542987508794829*^9, 3.54299811534949*^9, 
   3.543000005144605*^9, 3.543010414191695*^9, 3.543010723511387*^9, 
   3.5430109060098257`*^9, 3.5430794549095945`*^9, 3.5430796542313223`*^9, 
   3.543080314354065*^9, 3.5431060323959665`*^9, 3.5431062576918526`*^9, 
   3.543319480250066*^9, 3.5433199801666594`*^9, 3.543363882207965*^9, 
   3.543861911578001*^9, 3.5452658956420765`*^9, 3.5452666783563175`*^9, 
   3.545769060242565*^9, 3.5472214458588266`*^9, 3.547228549783148*^9, 
   3.547229429571469*^9, 3.5472298861825857`*^9, 3.547229982106072*^9, {
   3.5472308238762183`*^9, 3.547230839719125*^9}, 3.5472322592233157`*^9, 
   3.547242305745175*^9, 3.547309403848736*^9, 3.5474027940990753`*^9, 
   3.547403256522524*^9, 3.547403528880102*^9, 3.547491555743951*^9, 
   3.5477686373495517`*^9, 3.547830458555083*^9, 3.547899862968788*^9, 
   3.5515414633768177`*^9, 3.552650640941759*^9, 3.5526529843727956`*^9, 
   3.552828546064658*^9, 3.55293260292737*^9, 3.5563180373461323`*^9, 
   3.5563255301121635`*^9, 3.556327335399666*^9, 3.556385650958145*^9, 
   3.5568337050580587`*^9, 3.5813374282246614`*^9, 3.5880207914585166`*^9, 
   3.5926615956674366`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Linearized Hip Position", "Subsubsection",
 CellChangeTimes->{{3.52069721892881*^9, 3.520697223591626*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"outputsHip", "=", 
  RowBox[{"ParallelSimplify", "[", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      RowBox[{"Linearize", "[", 
       RowBox[{"#1", ",", 
        RowBox[{"Flatten", "[", "qe", "]"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "9", "}"}]}], "]"}]}], "]"}], "&"}], ")"}], "/@", 
    "outputsHip"}], "]"}]}]], "Input",
 CellChangeTimes->{{3.52069723201585*^9, 3.520697274076376*^9}, 
   3.520707794253764*^9, 3.541814527975404*^9, {3.5418145852846823`*^9, 
   3.541814587332799*^9}, {3.5427314865542526`*^9, 3.542731492669463*^9}}],

Cell[BoxData[
 RowBox[{"-", 
  FractionBox[
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"641", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
      RowBox[{"321", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], ")"}]}], 
   "100000"]}]], "Output",
 CellChangeTimes->{
  3.5425497359147964`*^9, 3.542556630448261*^9, 3.5425571432120185`*^9, 
   3.5425692743866773`*^9, 3.5425693391043787`*^9, {3.542569536136648*^9, 
   3.5425695449911547`*^9}, 3.542731351803215*^9, {3.542731489097057*^9, 
   3.5427315019982796`*^9}, 3.542731794615593*^9, 3.5427336987944794`*^9, 
   3.5427344131008635`*^9, 3.5427344605638666`*^9, 3.5427345879661293`*^9, 
   3.5427347303286*^9, 3.542734864861596*^9, 3.5427354235954947`*^9, 
   3.5427361540195303`*^9, 3.5427362586080327`*^9, 3.5427367945830946`*^9, 
   3.5427368597046385`*^9, 3.5429006643255515`*^9, 3.542902142803139*^9, 
   3.542904628812645*^9, 3.542907084212316*^9, 3.542931219471446*^9, 
   3.5429313487548404`*^9, 3.542985317182476*^9, 3.54298750881283*^9, 
   3.542998115367491*^9, 3.543000005160205*^9, 3.5430104142176967`*^9, 
   3.5430107235383887`*^9, 3.543010906033827*^9, 3.543079455003195*^9, 
   3.5430796542573233`*^9, 3.543080314392068*^9, 3.5431060324149675`*^9, 
   3.5431062577108536`*^9, 3.5433194802610664`*^9, 3.5433199801806602`*^9, 
   3.543363882217965*^9, 3.543861911618003*^9, 3.5452658956770787`*^9, 
   3.5452666783653173`*^9, 3.5457690602485657`*^9, 3.5472214458988285`*^9, 
   3.5472285498311505`*^9, 3.547229429608471*^9, 3.5472298861935863`*^9, 
   3.547229982161075*^9, {3.54723082390022*^9, 3.547230839743126*^9}, 
   3.547232259241317*^9, 3.547242305860175*^9, 3.5473094038787374`*^9, 
   3.5474027941440773`*^9, 3.5474032565495253`*^9, 3.547403528930105*^9, 
   3.5474915557929535`*^9, 3.5477686373785534`*^9, 3.547830458624087*^9, 
   3.54789986299879*^9, 3.551541463408018*^9, 3.552650640969761*^9, 
   3.552652984400797*^9, 3.5528285461106606`*^9, 3.552932602944371*^9, 
   3.5563180374081354`*^9, 3.5563255301781673`*^9, 3.556327335452669*^9, 
   3.5563856510201483`*^9, 3.556833705084621*^9, 3.581337428268664*^9, 
   3.5880207915045195`*^9, 3.592661595680437*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Combine Outputs", "Subsection",
 CellChangeTimes->{{3.528115166738313*^9, 3.528115169609477*^9}, 
   3.5386926823882256`*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"outputs", "=", 
  RowBox[{"{", 
   RowBox[{
   "outputsSTKnee", ",", "outputsNSKnee", ",", "outputsNSLeg", ",", 
    "outputsTorso", ",", "outputsNSfoot"}], "}"}]}]], "Input",
 CellChangeTimes->{{3.5275415458126264`*^9, 3.5275415621225595`*^9}, {
   3.5377233107073865`*^9, 3.5377233149349937`*^9}, {3.5377264840866785`*^9, 
   3.5377264863468075`*^9}, {3.5377412779480343`*^9, 
   3.5377412811872196`*^9}, {3.537798811970521*^9, 3.5377988169313297`*^9}, {
   3.538484764005954*^9, 3.5384847749571733`*^9}, {3.538485223115761*^9, 
   3.538485279135459*^9}, {3.5386927229205437`*^9, 3.5386927360152926`*^9}, {
   3.5418170325286565`*^9, 3.541817079150323*^9}, 3.5418178958500357`*^9, {
   3.541817998421902*^9, 3.5418180037492065`*^9}, {3.541872907298568*^9, 
   3.541872912524577*^9}, {3.5418729485294404`*^9, 3.541872973442684*^9}, {
   3.5418730267791777`*^9, 3.54187302813638*^9}, {3.5424093905391226`*^9, 
   3.5424093907951374`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", 
   RowBox[{
    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], ",", 
   RowBox[{
    RowBox[{"-", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "+", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
    FractionBox[
     RowBox[{"320", " ", 
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "641"]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
    RowBox[{
     SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->{{3.5418729743942857`*^9, 3.541873028713581*^9}, 
   3.5424714620397334`*^9, 3.542472566096945*^9, 3.542480030009946*^9, 
   3.5424840819751244`*^9, 3.542494457259405*^9, 3.5425426489112167`*^9, 
   3.542543015687195*^9, 3.542549744509288*^9, 3.542550421006981*^9, 
   3.5425505202516575`*^9, 3.542556630468261*^9, 3.5425571432320185`*^9, 
   3.5425692744096785`*^9, 3.54256933913038*^9, 3.5425695481243343`*^9, 
   3.5427313569512243`*^9, {3.542731506194687*^9, 3.5427315325275335`*^9}, 
   3.5427317948807936`*^9, 3.542733698817481*^9, 3.542734413140864*^9, 
   3.5427344605978675`*^9, 3.542734588010129*^9, 3.5427347303406005`*^9, 
   3.5427348649266*^9, 3.542735423633497*^9, 3.5427361540535326`*^9, 
   3.5427362586440353`*^9, 3.5427367946230965`*^9, 3.5427368597486415`*^9, {
   3.542900671860365*^9, 3.5429006922652006`*^9}, 3.542902142839141*^9, 
   3.5429046288282447`*^9, 3.542907084243516*^9, 3.5429312194934473`*^9, 
   3.5429313487848425`*^9, 3.542985317203477*^9, 3.5429875088338313`*^9, 
   3.542998115387492*^9, 3.543000005175805*^9, 3.543010414252699*^9, 
   3.5430107235683904`*^9, 3.5430109060568285`*^9, 3.5430794550499945`*^9, 
   3.543079654285325*^9, 3.543080314420069*^9, 3.5431060324359684`*^9, 
   3.543106257730855*^9, 3.543319480274067*^9, 3.543319980191661*^9, 
   3.543363882230966*^9, 3.5438619116590056`*^9, 3.545265895713081*^9, 
   3.5452666783753185`*^9, 3.545769060256566*^9, 3.54722144592183*^9, 
   3.547228549883154*^9, 3.5472294296504736`*^9, 3.547229886206587*^9, 
   3.547229982213078*^9, {3.547230823924221*^9, 3.5472308397681274`*^9}, 
   3.5472322592593174`*^9, 3.547242305955175*^9, 3.5473094039357405`*^9, 
   3.5474027942320824`*^9, 3.5474032565695267`*^9, 3.547403528982108*^9, 
   3.5474915558439565`*^9, 3.547768637408555*^9, 3.547830458663089*^9, 
   3.5478998630287914`*^9, 3.551541463486018*^9, 3.552650640993762*^9, 
   3.5526529844327993`*^9, 3.552828546137662*^9, 3.552932602962372*^9, 
   3.5563180374631386`*^9, 3.5563255302331705`*^9, 3.556327335506672*^9, 
   3.5563856510741515`*^9, 3.556833705111574*^9, 3.581337428308666*^9, 
   3.5880207915665226`*^9, 3.5926615957034388`*^9}]
}, Open  ]]
}, Open  ]]
}, Closed]],

Cell[CellGroupData[{

Cell["feedback control", "Section",
 CellChangeTimes->{{3.513532015025627*^9, 3.513532018772655*^9}}],

Cell["we will make use of the chain rule as much as possible", "Text",
 CellChangeTimes->{{3.513534246341532*^9, 3.513534256526356*^9}, {
  3.513534308611278*^9, 3.513534359658739*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"\[Chi]", "=", 
   RowBox[{"Join", "[", 
    RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"d\[Chi]", "=", 
   RowBox[{"D", "[", 
    RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533323577435*^9, 3.513533331092033*^9}, {
  3.513533381181343*^9, 3.513533384041068*^9}, {3.513534180527118*^9, 
  3.513534186497912*^9}, {3.513534242697953*^9, 3.513534243136149*^9}, {
  3.542549960773658*^9, 3.5425499664209805`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["p", "hip"], "=", " ", 
   RowBox[{
    RowBox[{
     SubscriptBox["g", "ship"], "[", "\[Theta]", "]"}], 
    "\[LeftDoubleBracket]", 
    RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["p", "hipdot"], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{"D", "[", 
     RowBox[{
      SubscriptBox["p", "hip"], ",", "t"}], "]"}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5418677518567996`*^9, 3.541867764212021*^9}, 
   3.5418677993276825`*^9, 3.5418678599337893`*^9, 3.5425499207583685`*^9, {
   3.5425696091458244`*^9, 3.5425696344872737`*^9}, 3.5430988121938877`*^9, {
   3.5472299573106537`*^9, 3.54722995951578*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{
    SubscriptBox["deltap", "hip"], " ", "=", " ", 
    RowBox[{"ParallelSimplify", "[", "outputsHip", "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     SubscriptBox["deltap", "hipdot"], "  ", "=", " ", 
     RowBox[{"ParallelSimplify", "[", 
      RowBox[{"D", "[", 
       RowBox[{
        SubscriptBox["deltap", "hip"], ",", "t"}], "]"}], "]"}]}], ";"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["deltap", "hipdot"], " ", "=", " ", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
    "outputsHip"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.520119821955995*^9, 3.5201198226710362`*^9}, {
   3.5201773456230736`*^9, 3.52017734590409*^9}, 3.541817126077007*^9, {
   3.541817168530435*^9, 3.541817169722503*^9}, 3.5418172314960365`*^9, {
   3.541817283518012*^9, 3.541817293021556*^9}, {3.5418677371771736`*^9, 
   3.541867744883587*^9}, 3.5418678157233114`*^9, {3.5418678648009977`*^9, 
   3.5418678661426*^9}, 3.542549928791828*^9, {3.5425499818768644`*^9, 
   3.5425499848290334`*^9}, {3.5474027463453436`*^9, 
   3.5474027532607393`*^9}, {3.547403064320531*^9, 3.547403111617236*^9}, {
   3.5474035013675284`*^9, 3.547403517212435*^9}, {3.547403602136292*^9, 
   3.5474036075616026`*^9}}],

Cell["time-invariant parameterization:", "Text",
 CellChangeTimes->{{3.513532550684887*^9, 3.513532556202501*^9}, {
  3.513532644709088*^9, 3.513532645133727*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"fb", "=", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
    FractionBox[
     RowBox[{
      SubscriptBox["deltap", "hip"], "-", 
      RowBox[{"p", "[", "1", "]"}]}], 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}]], "Input",
 CellChangeTimes->{{3.513532403934621*^9, 3.513532467910504*^9}, {
   3.513532514383117*^9, 3.513532544860577*^9}, {3.51353289142399*^9, 
   3.513532893553784*^9}, {3.513533036340894*^9, 3.513533036810536*^9}, {
   3.513533928303517*^9, 3.513533928507984*^9}, {3.513534006139525*^9, 
   3.513534007303687*^9}, {3.51394646342299*^9, 3.513946465979686*^9}, {
   3.513946518081392*^9, 3.513946518637576*^9}, {3.515336393379112*^9, 
   3.515336393434178*^9}, {3.5158551745864763`*^9, 3.515855177569647*^9}, 
   3.5201773758658037`*^9, 3.520186024265464*^9, 3.520443576003234*^9, 
   3.5204490394407244`*^9, 3.5207126311878996`*^9, 3.5223564440908136`*^9, 
   3.5224291342898664`*^9, 3.5418171514924603`*^9, {3.541867770374032*^9, 
   3.5418677711852336`*^9}, 3.5425499335671015`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
   FractionBox[
    RowBox[{
     RowBox[{"-", 
      RowBox[{"p", "[", "1", "]"}]}], "-", 
     FractionBox[
      RowBox[{"127", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"641", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{"321", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], ")"}]}], 
      "100000"]}], 
    RowBox[{"a", "[", 
     RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]], "Output",
 CellChangeTimes->{
  3.542549933927122*^9, 3.542556630578261*^9, 3.542557143332019*^9, 
   3.5427315496563635`*^9, 3.5427317949587936`*^9, 3.5427336988664837`*^9, 
   3.5427344132208643`*^9, 3.542734460674872*^9, 3.542734588091134*^9, 
   3.542734730379603*^9, 3.5427348650146027`*^9, 3.542735423701501*^9, 
   3.5427361541215363`*^9, 3.542736258712039*^9, 3.542736794715102*^9, 
   3.542736859819646*^9, 3.5429007472191706`*^9, 3.542902142894144*^9, 
   3.542904628875045*^9, 3.542907084274716*^9, 3.54293121954145*^9, 
   3.542931348844846*^9, 3.54298531724948*^9, 3.542987508881834*^9, 
   3.542998115460496*^9, 3.543000005238205*^9, 3.5430104143157024`*^9, 
   3.543010723666396*^9, 3.5430109061178317`*^9, 3.543079455127995*^9, 
   3.54307965436333*^9, 3.5430803144930735`*^9, 3.5431062799191236`*^9, 
   3.54331948032007*^9, 3.5433199802286625`*^9, 3.5433638822729683`*^9, 
   3.54386191173701*^9, 3.545265895805086*^9, 3.5452666784103203`*^9, 
   3.5457690602875676`*^9, 3.547221445989834*^9, 3.5472285500441628`*^9, 
   3.5472294297614794`*^9, 3.5472298863205934`*^9, 3.547229982357086*^9, {
   3.5472308240292273`*^9, 3.5472308398391314`*^9}, 3.5472322593213215`*^9, 
   3.547242306275175*^9, 3.547309404010745*^9, 3.5474027943610897`*^9, 
   3.547403256636531*^9, 3.5474035290791135`*^9, 3.547491555933962*^9, 
   3.54776863750156*^9, 3.5478304587460938`*^9, 3.547899863129797*^9, 
   3.5515414635640182`*^9, 3.5526506410617657`*^9, 3.552652984499803*^9, 
   3.5528285462126665`*^9, 3.5529326030323763`*^9, 3.5563180375971465`*^9, 
   3.5563255303681784`*^9, 3.55632733565168*^9, 3.5563856511861577`*^9, 
   3.556833705230879*^9, 3.581337428430673*^9, 3.5880207916545277`*^9, 
   3.5926615957834435`*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[Sigma]y", "=", 
   RowBox[{
    RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158620968964095`*^9, 3.5158621015066733`*^9}, {
   3.51586213624666*^9, 3.515862144043106*^9}, 3.5158641571459265`*^9, 
   3.541817172393656*^9}],

Cell[CellGroupData[{

Cell[TextData[{
 "calculate ",
 Cell[BoxData[
  FormBox[
   SubscriptBox["y", "d"], TraditionalForm]]],
 " and its derivatives"
}], "Subsubsection",
 CellChangeTimes->{{3.513534419031216*^9, 3.513534436458221*^9}, {
  3.515862063545502*^9, 3.515862065419609*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
   RowBox[{
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "1"}], "]"}], " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "2"}], "]"}], " ", 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "3"}], "]"}], " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "2"}], "]"}], " ", 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
     RowBox[{"Exp", "[", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"i", ",", "4"}], "]"}], 
       RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
    RowBox[{"a", "[", 
     RowBox[{"i", ",", "5"}], "]"}]}]}], ";"}]], "Input",
 CellChangeTimes->{{3.515860352077612*^9, 3.5158603885236964`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["y", 
    RowBox[{"d", ",", "1"}]], " ", "=", " ", 
   RowBox[{"a", "[", 
    RowBox[{"1", ",", "1"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"d", ",", "2"}]], " ", "=", " ", 
  RowBox[{
   RowBox[{"Transpose", "[", 
    RowBox[{"{", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", " ", "2", ",", 
         RowBox[{"ndof", "-", "1"}]}], "}"}]}], "]"}], "}"}], "]"}], "/.", 
   "fb"}]}]}], "Input",
 CellChangeTimes->{{3.515872287951982*^9, 3.5158723015107574`*^9}, {
   3.5158723371567965`*^9, 3.5158723885397353`*^9}, {3.515886208937134*^9, 
   3.51588621787595*^9}, {3.5158865763305807`*^9, 3.515886577094982*^9}, {
   3.516754046543599*^9, 3.5167540468006134`*^9}, {3.5418171856264133`*^9, 
   3.5418171931528435`*^9}, 3.541817853189595*^9, {3.541873185268257*^9, 
   3.54187319278747*^9}, {3.5424715037230062`*^9, 3.542471518199832*^9}, {
   3.542549948413951*^9, 3.542549954974326*^9}, {3.542738328330385*^9, 
   3.5427383289803867`*^9}, {3.5429009712667646`*^9, 3.542900984230387*^9}, 
   3.5429020455253067`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"641", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                RowBox[{"321", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], ")"}]}],
              "100000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"641", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                RowBox[{"321", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], ")"}]}],
              "100000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"641", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                RowBox[{"321", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], ")"}]}],
              "100000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"641", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                RowBox[{"321", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], ")"}]}],
              "100000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"6", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"6", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"641", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                RowBox[{"321", " ", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], ")"}]}],
              "100000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"641", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
                  RowBox[{"321", " ", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], 
                 ")"}]}], "100000"]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.5429020462273073`*^9, 3.5429021429551477`*^9, 3.542904628906245*^9, 
   3.5429070843059163`*^9, 3.5429312195984535`*^9, 3.542931348883848*^9, 
   3.5429853172834816`*^9, 3.5429875089118357`*^9, 3.5429981154854975`*^9, 
   3.543000005253805*^9, 3.5430104143517046`*^9, 3.543010723704398*^9, 
   3.5430109061538334`*^9, 3.5430794551903954`*^9, 3.543079654405332*^9, 
   3.543080314532076*^9, 3.5431062799611263`*^9, 3.5433194803510714`*^9, 
   3.5433199802426634`*^9, 3.54336388229797*^9, 3.5438619118180146`*^9, 
   3.5452658958530893`*^9, 3.545266678426321*^9, 3.5457690602975683`*^9, 
   3.547221446037837*^9, 3.547228550111167*^9, 3.5472294298104825`*^9, 
   3.5472298863405943`*^9, 3.5472299824310904`*^9, {3.5472308240842304`*^9, 
   3.5472308398751335`*^9}, 3.5472322593503227`*^9, 3.5472423064101753`*^9, 
   3.547309404062748*^9, 3.547402794419093*^9, 3.5474032566635323`*^9, 
   3.5474035291401167`*^9, 3.547491555990965*^9, 3.547768637788577*^9, 
   3.547830458808097*^9, 3.5478998631738*^9, 3.551541463626418*^9, 
   3.5526506411277695`*^9, 3.5526529845358047`*^9, 3.5528285462646694`*^9, 
   3.5529326030593777`*^9, 3.5563180376851516`*^9, 3.5563255304661837`*^9, 
   3.556327335721684*^9, 3.5563856512531614`*^9, 3.5568337054499817`*^9, 
   3.581337428493677*^9, 3.588020791769534*^9, 3.592661595813445*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"d", ",", "1"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"d", ",", "2"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533175133389*^9, 3.513533227600235*^9}, {
   3.513534447833279*^9, 3.513534448428582*^9}, {3.513534581373526*^9, 
   3.51353459872282*^9}, {3.513534853366941*^9, 3.513534855172635*^9}, {
   3.513874463242996*^9, 3.513874465281417*^9}, {3.5158616236643424`*^9, 
   3.5158616650997124`*^9}, {3.5158617380638857`*^9, 3.5158617555558863`*^9}, 
   3.515862210846927*^9, {3.5158724149302444`*^9, 3.5158724662411795`*^9}, {
   3.5418172137210197`*^9, 3.5418172196563597`*^9}, {3.542902085804579*^9, 
   3.542902095492196*^9}, 3.5429021334782624`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"d", ",", "1"}]], "=", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
    "ParallelSimplify"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"d", ",", "2"}]], "=", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
    "ParallelSimplify"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158617931210346`*^9, 3.5158618378455925`*^9}, 
   3.5158620205530434`*^9, 3.515862208155773*^9, {3.515872496206894*^9, 
   3.5158725099996824`*^9}, {3.5418172280728407`*^9, 3.541817229040896*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["calculate actual kinematics outputs and Jacobians", "Subsubsection",
 CellChangeTimes->{{3.513532922993929*^9, 3.513532938890856*^9}, {
  3.513534921064935*^9, 3.513534931010012*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"a", ",", "1"}]], "=", 
  RowBox[{
   SubscriptBox["\[PartialD]", "t"], "outputsHip"}]}], "\n", 
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"a", ",", "2"}]], "=", 
  RowBox[{"Transpose", "[", 
   RowBox[{"{", 
    RowBox[{"Table", "[", 
     RowBox[{"outputs", "\[LeftDoubleBracket]", 
      RowBox[{"1", ";;", "All"}], "\[RightDoubleBracket]"}], "]"}], "}"}], 
   "]"}]}]}], "Input",
 CellChangeTimes->{{3.513532586333204*^9, 3.513532677723161*^9}, {
   3.513532815648106*^9, 3.51353282124899*^9}, {3.513532861207259*^9, 
   3.513532869009141*^9}, {3.513532899901752*^9, 3.513532900419178*^9}, {
   3.51353311611145*^9, 3.513533116605757*^9}, 3.515521088606793*^9, {
   3.5155211643341246`*^9, 3.5155211667002597`*^9}, {3.5158526171702003`*^9, 
   3.515852621396442*^9}, {3.5158548842498703`*^9, 3.5158548875960617`*^9}, {
   3.5158551932005405`*^9, 3.5158551957126846`*^9}, {3.5158593451740203`*^9, 
   3.515859367792314*^9}, 3.51586024539751*^9, {3.5158611465420523`*^9, 
   3.515861146589055*^9}, 3.5158681409047847`*^9, {3.515869758310295*^9, 
   3.5158697588093233`*^9}, {3.515869821362901*^9, 3.515869822510967*^9}, {
   3.5158725172250957`*^9, 3.515872573737328*^9}, 3.5158863481049786`*^9, {
   3.515886571837773*^9, 3.5158865734445753`*^9}, {3.51595325150783*^9, 
   3.5159532575301747`*^9}, {3.5167083309077144`*^9, 
   3.5167083312677355`*^9}, {3.5167110381765614`*^9, 3.5167110528944035`*^9}, 
   3.5167111385673037`*^9, {3.5201093866961327`*^9, 3.520109388523237*^9}, 
   3.5201094274054613`*^9, 3.5201773866844225`*^9, {3.520437239317703*^9, 
   3.5204372535225153`*^9}, {3.5204490557876596`*^9, 
   3.5204490607579436`*^9}, {3.5207159029790354`*^9, 3.520715937276997*^9}, 
   3.541817245750852*^9, 3.5418172995169272`*^9, {3.541867891742245*^9, 
   3.5418678924130464`*^9}, 3.542471618008807*^9, 3.542549990660367*^9, {
   3.5429009324850965`*^9, 3.5429009466031213`*^9}, 3.547403543948964*^9, {
   3.5474035759387937`*^9, 3.547403591363676*^9}}],

Cell[BoxData[
 RowBox[{"-", 
  FractionBox[
   RowBox[{"127", " ", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{"641", " ", 
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "1"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}]}], "+", 
      RowBox[{"321", " ", 
       RowBox[{
        SuperscriptBox[
         SubscriptBox["\[Theta]", "2"], "\[Prime]",
         MultilineFunction->None], "[", "t", "]"}]}]}], ")"}]}], 
   "100000"]}]], "Output",
 CellChangeTimes->{
  3.54254999106339*^9, 3.5425504453623743`*^9, 3.54255052256079*^9, 
   3.542556630758261*^9, 3.5425571434720187`*^9, 3.5427315618555846`*^9, 
   3.542731796815197*^9, 3.5427336996047244`*^9, 3.5427344134508643`*^9, 
   3.5427344608628826`*^9, 3.5427345883061466`*^9, 3.5427347305156107`*^9, 
   3.5427348652086143`*^9, 3.5427354255754027`*^9, 3.542736154291546*^9, 
   3.5427362588780484`*^9, 3.542736794888112*^9, 3.542736859997656*^9, 
   3.542900747265971*^9, {3.542900934216699*^9, 3.542900947055522*^9}, 
   3.5429009892847958`*^9, 3.5429021430681543`*^9, 3.542904629015445*^9, 
   3.5429070843839164`*^9, 3.5429312204975047`*^9, 3.542931348997854*^9, 
   3.542985318794568*^9, 3.542987509017842*^9, 3.542998116678566*^9, 
   3.5430000053630056`*^9, 3.543010724923468*^9, 3.5430109062658405`*^9, 
   3.5430794570467987`*^9, 3.5430796550513687`*^9, 3.543080314697085*^9, 
   3.5431062809771843`*^9, 3.5433194804610777`*^9, 3.5433199803426695`*^9, 
   3.5433638832920265`*^9, 3.5438619136301184`*^9, 3.5452658968351436`*^9, 
   3.5452666785203266`*^9, 3.5457690611716185`*^9, 3.547221446953889*^9, 
   3.547228550237174*^9, 3.5472294298704863`*^9, 3.5472298872906485`*^9, 
   3.547229982570099*^9, {3.547230824213238*^9, 3.547230840007141*^9}, 
   3.547232260257375*^9, 3.547242313702533*^9, 3.5473094050018015`*^9, 
   3.547402795302144*^9, 3.547403256777539*^9, {3.5474035292741246`*^9, 
   3.547403557896762*^9}, 3.547491556865015*^9, 3.547768639875696*^9, 
   3.5478304597071486`*^9, 3.547899863993847*^9, 3.55154146445322*^9, 
   3.552650641876813*^9, 3.552652984647812*^9, 3.5528285470817165`*^9, 
   3.552932603923427*^9, 3.5563180395682592`*^9, 3.5563255325383024`*^9, 
   3.556327336555732*^9, 3.556385653130269*^9, 3.556833707000087*^9, 
   3.58133743466703*^9, 3.588020797393856*^9, 3.5926615964834833`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
     FractionBox[
      RowBox[{"320", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "641"]}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.54254999106339*^9, 3.5425504453623743`*^9, 3.54255052256079*^9, 
   3.542556630758261*^9, 3.5425571434720187`*^9, 3.5427315618555846`*^9, 
   3.542731796815197*^9, 3.5427336996047244`*^9, 3.5427344134508643`*^9, 
   3.5427344608628826`*^9, 3.5427345883061466`*^9, 3.5427347305156107`*^9, 
   3.5427348652086143`*^9, 3.5427354255754027`*^9, 3.542736154291546*^9, 
   3.5427362588780484`*^9, 3.542736794888112*^9, 3.542736859997656*^9, 
   3.542900747265971*^9, {3.542900934216699*^9, 3.542900947055522*^9}, 
   3.5429009892847958`*^9, 3.5429021430681543`*^9, 3.542904629015445*^9, 
   3.5429070843839164`*^9, 3.5429312204975047`*^9, 3.542931348997854*^9, 
   3.542985318794568*^9, 3.542987509017842*^9, 3.542998116678566*^9, 
   3.5430000053630056`*^9, 3.543010724923468*^9, 3.5430109062658405`*^9, 
   3.5430794570467987`*^9, 3.5430796550513687`*^9, 3.543080314697085*^9, 
   3.5431062809771843`*^9, 3.5433194804610777`*^9, 3.5433199803426695`*^9, 
   3.5433638832920265`*^9, 3.5438619136301184`*^9, 3.5452658968351436`*^9, 
   3.5452666785203266`*^9, 3.5457690611716185`*^9, 3.547221446953889*^9, 
   3.547228550237174*^9, 3.5472294298704863`*^9, 3.5472298872906485`*^9, 
   3.547229982570099*^9, {3.547230824213238*^9, 3.547230840007141*^9}, 
   3.547232260257375*^9, 3.547242313702533*^9, 3.5473094050018015`*^9, 
   3.547402795302144*^9, 3.547403256777539*^9, {3.5474035292741246`*^9, 
   3.547403557896762*^9}, 3.547491556865015*^9, 3.547768639875696*^9, 
   3.5478304597071486`*^9, 3.547899863993847*^9, 3.55154146445322*^9, 
   3.552650641876813*^9, 3.552652984647812*^9, 3.5528285470817165`*^9, 
   3.552932603923427*^9, 3.5563180395682592`*^9, 3.5563255325383024`*^9, 
   3.556327336555732*^9, 3.556385653130269*^9, 3.556833707000087*^9, 
   3.58133743466703*^9, 3.588020797393856*^9, 3.5926615964844837`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"a", ",", "1"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["Dy", 
    RowBox[{"a", ",", "2"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533234344025*^9, 3.5135333201722*^9}, {
   3.513533374269628*^9, 3.513533375964947*^9}, 3.513534856986833*^9, 
   3.513874036445271*^9, {3.513947979870945*^9, 3.513948005409799*^9}, 
   3.515521160512906*^9, 3.5158669774602394`*^9, {3.515872580275702*^9, 
   3.515872610360423*^9}, {3.5418173419473543`*^9, 3.541817342490385*^9}, {
   3.541818019486107*^9, 3.5418180263655005`*^9}, {3.547403480977362*^9, 
   3.5474034890178223`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"a", ",", "1"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["DLfy", 
    RowBox[{"a", ",", "2"}]], "=", 
   RowBox[{"ParallelSimplify", "[", 
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
     RowBox[{"Flatten", "[", 
      RowBox[{
       SubscriptBox["Dy", 
        RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.513533411525342*^9, 3.513533467774484*^9}, {
  3.513534859343821*^9, 3.513534864828146*^9}, {3.513874068222587*^9, 
  3.513874090913892*^9}, {3.513874132914815*^9, 3.513874149954311*^9}, {
  3.513874244874198*^9, 3.513874245301785*^9}, {3.513947988668254*^9, 
  3.513948001956114*^9}, {3.515521142307864*^9, 3.5155211535195055`*^9}, {
  3.515861859950857*^9, 3.515861881451087*^9}, {3.515861991803399*^9, 
  3.5158620021649914`*^9}, {3.5158656112430964`*^9, 3.515865614533284*^9}, {
  3.515872634222788*^9, 3.5158726502537045`*^9}, {3.5201204072844744`*^9, 
  3.52012040879156*^9}, {3.541817343923467*^9, 3.5418173443544917`*^9}, {
  3.5418180334619064`*^9, 3.541818035685034*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["write the control files to disk", "Subsubsection",
 CellChangeTimes->{{3.513533556726437*^9, 3.513533560633779*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[Sigma]y", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.515862148936386*^9, 3.5158621546717143`*^9}, {
   3.516708335792994*^9, 3.516708336128013*^9}, 3.5207161434147873`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
   3.516708336825053*^9, 3.516708337152072*^9}, 3.520716145044881*^9, {
   3.5418646945348907`*^9, 3.54186470388186*^9}, 3.5418679174978905`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hipdot"], " ", "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
   3.516708336825053*^9, 3.516708337152072*^9}, {3.5167541031348357`*^9, 
   3.516754114654495*^9}, {3.5203390769255333`*^9, 3.520339105876189*^9}, {
   3.520716149641144*^9, 3.5207161499701624`*^9}, 3.5418647183505516`*^9, 
   3.5418679204306955`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaphip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["deltap", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}],
      "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
   3.516708336825053*^9, 3.516708337152072*^9}, 3.520716145044881*^9, {
   3.5418646945348907`*^9, 3.54186470388186*^9}, {3.541867906983472*^9, 
   3.541867908106674*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaphip_dot_mat\>\"", "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["deltap", "hipdot"], " ", "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
   3.516708336825053*^9, 3.516708337152072*^9}, {3.5167541031348357`*^9, 
   3.516754114654495*^9}, {3.5203390769255333`*^9, 3.520339105876189*^9}, {
   3.520716149641144*^9, 3.5207161499701624`*^9}, 3.5418647183505516`*^9, {
   3.5418679127242823`*^9, 3.5418679132702827`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<pcom_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "COM"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158901103775215`*^9, 3.5158901154943304`*^9}, {
   3.515948100165904*^9, 3.515948106905116*^9}, {3.5159487206466036`*^9, 
   3.5159487213174047`*^9}, {3.51594878707152*^9, 3.5159487941383324`*^9}, {
   3.515953230430625*^9, 3.51595323192271*^9}, {3.515953761686011*^9, 
   3.515953766147266*^9}, 3.515985484570779*^9, {3.5167083379371166`*^9, 
   3.5167083382401342`*^9}, {3.5207162236953793`*^9, 3.5207162275425997`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158726672456765`*^9, 3.51587268496169*^9}, {
   3.5167083389361734`*^9, 3.516708341929345*^9}, 3.5207162296587205`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, 
   3.513535978245256*^9, {3.5139480491379*^9, 3.513948080396936*^9}, {
   3.513948113545785*^9, 3.513948129942129*^9}, 3.5158619633557715`*^9, {
   3.5158726892259336`*^9, 3.515872692325111*^9}, {3.5167083517449064`*^9, 
   3.5167083521129274`*^9}, 3.5207162311398053`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727027727084`*^9, 3.515872707855999*^9}, {
   3.5167083527929664`*^9, 3.5167083530889835`*^9}, 3.5207162325458856`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533604687178*^9}, {
   3.513535690806333*^9, 3.513535693339207*^9}, 3.513535980161328*^9, 
   3.51586196000458*^9, {3.515872712110243*^9, 3.5158727149254036`*^9}, {
   3.516708353801024*^9, 3.5167083540880404`*^9}, 3.5207163273033056`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515861942956605*^9, 3.515861955946348*^9}, {
   3.515874032033738*^9, 3.515874045405503*^9}, {3.5167083552411065`*^9, 
   3.5167083555441236`*^9}, 3.520716332000574*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515861942956605*^9, 3.515861955946348*^9}, {
   3.5158740329057875`*^9, 3.515874042230321*^9}, {3.5167083567361917`*^9, 
   3.5167083570722113`*^9}, 3.5207163337346735`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727241079288`*^9, 3.5158727304642925`*^9}, {
   3.516708359905373*^9, 3.5167083605204086`*^9}, 3.52071633560478*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513535713498742*^9, 3.513535727403304*^9}, {3.5158727332724533`*^9, 
   3.5158727361286163`*^9}, {3.5167083610644393`*^9, 3.516708361376457*^9}, 
   3.5207163401990433`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727417019353`*^9, 3.51587274563416*^9}, {
   3.5167083619924927`*^9, 3.5167083623125105`*^9}, 3.520716341684128*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533604687178*^9}, {
   3.513535690806333*^9, 3.51353572494454*^9}, {3.515872742920005*^9, 
   3.5158727481833057`*^9}, {3.5167083632325635`*^9, 3.5167083635285807`*^9}, 
   3.5207163448703103`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533607692186*^9}, 
   3.513534673091028*^9, {3.513535695836862*^9, 3.513535722564655*^9}, {
   3.513536332459772*^9, 3.513536336316357*^9}, {3.5158727571118164`*^9, 
   3.515872763041156*^9}, {3.516708364688647*^9, 3.516708364976663*^9}, 
   3.520716347555464*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727601249886`*^9, 3.5158727645362415`*^9}, {
   3.516708366824769*^9, 3.516708367128786*^9}, 3.520716352285734*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["PERL", "Subsection",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}}],

Cell["this next line assumes that perl is in your path", "Text",
 CellChangeTimes->{{3.513015230408543*^9, 3.513015241200741*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2mat_torso.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.504476276692404*^9, 3.504476301348216*^9}, {
   3.504478868125564*^9, 3.504478869967337*^9}, {3.504538702237328*^9, 
   3.504538704315194*^9}, {3.504631487786531*^9, 3.504631500259924*^9}, {
   3.504632214564033*^9, 3.504632215194798*^9}, {3.504635416937057*^9, 
   3.50463541912094*^9}, {3.504874639115547*^9, 3.504874639679282*^9}, 
   3.504876166859266*^9, {3.505578303460942*^9, 3.5055783039289427`*^9}, {
   3.512922890945364*^9, 3.512922891900902*^9}, {3.5129244660544987`*^9, 
   3.51292449464812*^9}, {3.512925468730114*^9, 3.512925469241001*^9}, {
   3.513011661780643*^9, 3.513011661926162*^9}, {3.51301497676734*^9, 
   3.513015001300451*^9}, {3.5158700422275343`*^9, 3.515870043613613*^9}, {
   3.515870412159693*^9, 3.5158704163519325`*^9}, {3.5158863672774124`*^9, 
   3.5158863722382207`*^9}}],

Cell[BoxData["2"], "Output",
 CellChangeTimes->{
  3.52502885606983*^9, 3.525040921934133*^9, 3.525053941373309*^9, 
   3.525221793759395*^9, 3.525222103045013*^9, 3.525222721424881*^9, 
   3.541818725848509*^9, 3.541819463605706*^9, 3.541863577571141*^9, 
   3.541868248835675*^9, 3.5418707977986183`*^9, 3.541873219432317*^9, 
   3.5424714793401637`*^9, 3.542471635995639*^9, 3.5424725848013773`*^9, 
   3.5424800332644787`*^9, 3.542484089400738*^9, 3.5424944590535073`*^9, 
   3.5425426494672484`*^9, 3.5425430170992756`*^9, 3.542550531704313*^9, 
   3.5425566310082617`*^9, 3.542557143672019*^9, 3.5427315752248077`*^9, 
   3.5427317972363977`*^9, 3.54273369990034*^9, 3.542734413813878*^9, 
   3.5427344612589054`*^9, 3.5427345887231703`*^9, 3.5427347307956266`*^9, 
   3.5427348654926305`*^9, 3.5427354259654255`*^9, 3.5427361546225643`*^9, 
   3.542736259164065*^9, 3.5427367952551327`*^9, 3.5427368603296747`*^9, 
   3.5429007474531713`*^9, 3.5429021432391644`*^9, {3.5429046012005963`*^9, 
   3.5429046291558456`*^9}, 3.5429070845243163`*^9, 3.542931220714517*^9, 
   3.542931349196866*^9, 3.542985319018581*^9, 3.542987509230854*^9, 
   3.5429981169315805`*^9, 3.543000005565806*^9, 3.543010725167482*^9, 
   3.543010906549856*^9, 3.543079457483599*^9, 3.5430796553773875`*^9, 
   3.5430803149751005`*^9, 3.5431062812612004`*^9, 3.54331948067409*^9, 
   3.543319980493678*^9, 3.543363883512039*^9, 3.5438619139721375`*^9, 
   3.545265897060157*^9, 3.5452666786643333`*^9, 3.545769061307626*^9, 
   3.547221447245906*^9, 3.5472285510342197`*^9, 3.5472294315425816`*^9, 
   3.547229887550664*^9, 3.5472299828491144`*^9, {3.5472308244662523`*^9, 
   3.5472308402591553`*^9}, 3.5472322606313963`*^9, 3.5472423150836105`*^9, 
   3.547309405294819*^9, 3.547402795492155*^9, 3.5474031418109627`*^9, 
   3.5474032569425483`*^9, 3.547403529461135*^9, 3.547490283946208*^9, 
   3.5474903468228045`*^9, 3.547490930258175*^9, 3.5474915570500255`*^9, 
   3.5477686419608154`*^9, 3.5478304599401617`*^9, 3.54789986422186*^9, 
   3.5515414654828215`*^9, 3.5526506420708237`*^9, 3.5526529848208213`*^9, 
   3.5528285473457313`*^9, 3.5529326041394396`*^9, 3.556318040058287*^9, 
   3.556325532936325*^9, 3.5563273369657555`*^9, 3.556385653650299*^9, 
   3.556833707476487*^9, 3.5813374391022835`*^9, 3.588020800679044*^9, 
   3.5926615967614994`*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Inverse Kinematics", "Section",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}, {
  3.5140469816745305`*^9, 3.5140469835466375`*^9}, {3.51405491908371*^9, 
  3.514054919326081*^9}, {3.5207282376405377`*^9, 3.520728240214685*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]], "Input",
 CellChangeTimes->{{3.5158974201226616`*^9, 3.515897422208781*^9}, 
   3.5159409950429645`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[CapitalDelta]", "=", " ", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "1"}], "}"}], 
     ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1", ",", "0"}], "}"}], 
     ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "1", ",", "0", ",", "0"}], "}"}], 
     ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0", ",", "0"}], "}"}], 
     ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
     ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"1", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
      "}"}]}], "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5126870727453375`*^9, 3.5126871090532956`*^9}, {
   3.512687198656415*^9, 3.5126872022367573`*^9}, {3.5126906830481005`*^9, 
   3.5126907051903095`*^9}, {3.514054842423344*^9, 3.514054877019777*^9}, 
   3.514054909726557*^9, {3.5158588592522273`*^9, 3.5158589038437777`*^9}, {
   3.5158839903865247`*^9, 3.5158839971019087`*^9}, 3.515940988613597*^9, {
   3.515941980195312*^9, 3.515942020051592*^9}, {3.5418175200275393`*^9, 
   3.541817569041343*^9}, {3.542572480931081*^9, 3.5425724974970284`*^9}, {
   3.542572688849973*^9, 3.5425727118242874`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"initialtime", "=", 
   RowBox[{"{", 
    RowBox[{"t", "\[Rule]", "0"}], "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158840455366793`*^9, 3.515884055622256*^9}, {
  3.5158841551359477`*^9, 3.5158841624563665`*^9}, {3.515891350156479*^9, 
  3.515891350585504*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"resetmap", "=", 
  RowBox[{"Module", "[", 
   RowBox[{
    RowBox[{"{", "newq", "}"}], ",", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"newq", "=", 
      RowBox[{"\[CapitalDelta]", ".", 
       RowBox[{"(", "q", ")"}]}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "0", "]"}], "->", 
        RowBox[{"newq", "[", 
         RowBox[{"[", 
          RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", 
         RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}]}]}], 
   "\[IndentingNewLine]", "]"}]}]], "Input",
 CellChangeTimes->{
  3.515889034698043*^9, {3.5158896543664856`*^9, 3.515889655282538*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "6"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->{{3.5429981735538187`*^9, 3.5429981875306187`*^9}, 
   3.542998247033022*^9, 3.543000005597006*^9, 3.5430107252084846`*^9, 
   3.5430109065888586`*^9, 3.5430794575303993`*^9, 3.543079655443391*^9, 
   3.543080315031104*^9, 3.5431062952339993`*^9, 3.5433194807020917`*^9, 
   3.543319980511679*^9, 3.5433638835330405`*^9, 3.543861914042142*^9, 
   3.545265897121159*^9, 3.5452666786813345`*^9, 3.5457690613166265`*^9, 
   3.547221447285908*^9, 3.5472285511042233`*^9, 3.547229431591584*^9, 
   3.547229887598666*^9, 3.5472299829201183`*^9, {3.547230824529256*^9, 
   3.5472308402981577`*^9}, 3.5472322607394023`*^9, 3.5472423152516203`*^9, 
   3.5473094053458214`*^9, 3.547402795564159*^9, 3.5474032569795504`*^9, 
   3.547403529585142*^9, 3.547491557143031*^9, 3.547768642026819*^9, 
   3.5478304600041656`*^9, 3.547899864283863*^9, 3.5515414655452223`*^9, 
   3.552650642137828*^9, 3.552652984886825*^9, 3.552828547412735*^9, 
   3.5529326041764417`*^9, 3.556318040141292*^9, 3.5563255330323305`*^9, 
   3.5563273370507603`*^9, 3.5563856537493043`*^9, 3.556833707575128*^9, 
   3.581337439181288*^9, 3.588020800772049*^9, 3.5926615968215027`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Parameterized Time Using the Output of the Hip", "Subsection",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.5133462255725746`*^9, 3.513346244405652*^9}, {
   3.51334632129305*^9, 3.513346341733219*^9}, 3.541863724648199*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"phipcondition", "=", 
   RowBox[{
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
       SubscriptBox["deltap", "hip"]}], "}"}], "/.", "constsubs"}], "/.", 
    RowBox[{"t", "\[Rule]", "0"}]}]}], ";"}]], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.515889672571527*^9, 3.515889674968664*^9}, {
   3.515890444251664*^9, 3.515890444569683*^9}, {3.515942148250924*^9, 
   3.515942174667435*^9}, {3.5159426119234447`*^9, 3.5159426332196627`*^9}, 
   3.515943242212495*^9, 3.515943756731924*^9, {3.5159855129044*^9, 
   3.5159855341526146`*^9}, {3.5207282787238874`*^9, 
   3.5207282828571243`*^9}, {3.5223565377914543`*^9, 
   3.5223565440782657`*^9}, {3.5224294884521236`*^9, 3.5224294923883486`*^9}, 
   3.5418176205752907`*^9, {3.541863711528576*^9, 3.5418637402170267`*^9}, {
   3.5418681819739575`*^9, 3.541868184017561*^9}, {3.54255055717977*^9, 
   3.5425505607789755`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"phipcondition", "/.", "resetmap"}], ";"}]], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.516751800532134*^9, 3.516751805196401*^9}, {
   3.516752237310117*^9, 3.5167522378981504`*^9}, 3.5417803634829464`*^9, {
   3.5418176337900467`*^9, 3.5418176399573994`*^9}, 3.541863724648199*^9, {
   3.542550563187114*^9, 3.542550566554306*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", "fb"}], 
  ";"}]], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{
  3.5417803665391216`*^9, 3.541863724648199*^9, {3.54255056854642*^9, 
   3.5425505708105497`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"tau", "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", 
         "fb"}], "/.", "\[InvisibleSpace]", "phipcondition"}], "/.", 
       "resetmap"}], "/.", "constsubs"}], "]"}]}], ";"}], 
  "\[IndentingNewLine]"}]], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.5158895462843037`*^9, 3.5158895925569506`*^9}, {
  3.515889789857235*^9, 3.5158897931634245`*^9}, {3.515889831234602*^9, 
  3.5158898935841684`*^9}, {3.5158904699201326`*^9, 3.5158904714572206`*^9}, {
  3.515891377005015*^9, 3.515891377386037*^9}, {3.5159855481004124`*^9, 
  3.5159855596250715`*^9}, {3.541863724648199*^9, 3.541863734538616*^9}, {
  3.542550572978674*^9, 3.542550585626397*^9}, {3.542572524552576*^9, 
  3.5425725278007617`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<tau\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"tau", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.},
 CellChangeTimes->{{3.515891747597212*^9, 3.5158917482732506`*^9}, 
   3.541863724648199*^9}]
}, Open  ]],

Cell["extract the (relative degree two) position-based outputs", "Text",
 CellChangeTimes->{{3.51405623270548*^9, 3.514056248417758*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   SubscriptBox["y", "2"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{"(", 
      RowBox[{
       SubscriptBox["y", 
        RowBox[{"a", ",", "2"}]], "-", 
       SubscriptBox["y", 
        RowBox[{"d", ",", "2"}]]}], ")"}], "/.", "\[InvisibleSpace]", 
     "phipcondition"}], "/.", "constsubs"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5159429875229278`*^9, 3.515943032690511*^9}, {
   3.51598557480694*^9, 3.5159855753639717`*^9}, 3.5417803800918965`*^9, {
   3.5418182025415773`*^9, 3.5418182054527435`*^9}, {3.542550632240063*^9, 
   3.542550661454734*^9}, {3.5427317084254417`*^9, 3.5427317184406595`*^9}, 
   3.542900896948234*^9, 3.5429010276764636`*^9}],

Cell["compute the pre- and post-impact values of the outputs", "Text",
 CellChangeTimes->{{3.514056343774789*^9, 3.514056355919777*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus", "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["y", "2"], "/.", "initialtime"}], "/.", "resetmap"}], "//", 
   "ParallelSimplify"}]}]], "Input",
 CellChangeTimes->{{3.5159428874992065`*^9, 3.5159429143387423`*^9}, {
  3.51594304105899*^9, 3.5159430447622013`*^9}, {3.542731726817874*^9, 
  3.5427317277070756`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     FractionBox[
      RowBox[{"320", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "641"], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"6", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"6", ",", "5"}], "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.525028890059209*^9, 3.525040955629686*^9, 3.525053974795913*^9, 
   3.525221827028521*^9, 3.525222134828111*^9, 3.525222752398978*^9, 
   3.5418176574153976`*^9, 3.5418180565302258`*^9, 3.541818746140669*^9, 
   3.541819510589393*^9, 3.54186375357065*^9, 3.5418682490540752`*^9, 
   3.541870798063819*^9, 3.5424716483508606`*^9, 3.5424726192462378`*^9, 
   3.5424727044379873`*^9, 3.542480033542492*^9, 3.542484106257968*^9, 
   3.542494459250519*^9, 3.5425426496772604`*^9, 3.5425430172862864`*^9, 
   3.5425505963940125`*^9, 3.5425506437967243`*^9, 3.5425566311782618`*^9, 
   3.5425571438320193`*^9, {3.5427317231674676`*^9, 3.542731728081476*^9}, 
   3.542731797485998*^9, 3.5427337000293474`*^9, 3.54273441401989*^9, 
   3.542734461445916*^9, 3.5427345889371824`*^9, 3.5427347308906317`*^9, 
   3.5427348657026424`*^9, 3.5427354261904383`*^9, 3.542736154790574*^9, 
   3.5427362593490753`*^9, 3.5427367954571447`*^9, 3.5427368604944816`*^9, 
   3.5427383348184905`*^9, 3.5427384006511097`*^9, 3.542738439507598*^9, 
   3.542900794783654*^9, 3.542900837075329*^9, 3.542900907213052*^9, 
   3.542901029782467*^9, 3.542901064071327*^9, 3.54290214334217*^9, 
   3.5429046292338457`*^9, 3.5429070845867167`*^9, 3.5429312208285236`*^9, 
   3.5429313492968717`*^9, 3.5429853191305876`*^9, 3.5429875093078585`*^9, 
   3.5429982834311037`*^9, 3.5430000056282063`*^9, 3.54301072530449*^9, 
   3.5430109066538625`*^9, 3.5430794577176*^9, 3.5430796555303965`*^9, 
   3.543080315117109*^9, 3.5431062953110037`*^9, 3.543319480737094*^9, 
   3.5433199805466814`*^9, 3.5433638836080446`*^9, 3.5438619141791496`*^9, 
   3.5452658972331657`*^9, 3.545266678708336*^9, 3.5457690613856306`*^9, 
   3.5472214473809137`*^9, 3.547228552756318*^9, 3.54722943238663*^9, 
   3.5472298877626762`*^9, 3.547229983005123*^9, {3.54723082460326*^9, 
   3.5472308403431606`*^9}, 3.547232260856409*^9, 3.5472423158456516`*^9, 
   3.547309405439827*^9, 3.5474027956781654`*^9, 3.5474032570445538`*^9, 
   3.547403529659147*^9, 3.5474915572650375`*^9, 3.547768642182828*^9, 
   3.5478304601111717`*^9, 3.5478998643778687`*^9, 3.551541465638822*^9, 
   3.5526506422268324`*^9, 3.552652984951829*^9, 3.552828547518741*^9, 
   3.5529326042614465`*^9, 3.556318040336303*^9, 3.556325533230342*^9, 
   3.5563273371577663`*^9, 3.5563856539213142`*^9, 3.556833707767892*^9, 
   3.5813374399363313`*^9, 3.5880208018371105`*^9, 3.592661596912508*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "\[Equal]", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{"{", 
         SubscriptBox["yd", "i"], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], ")"}], "/.", 
    "initialtime"}], "/.", "resetmap"}], "//", "ParallelSimplify"}]], "Input",\

 CellChangeTimes->{{3.5427382959935493`*^9, 3.542738309960634*^9}, {
  3.542738360310828*^9, 3.5427383841064353`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"{", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{
      FractionBox[
       RowBox[{"320", " ", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "641"], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"-", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}]}], "}"}], 
  "\[Equal]", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{"{", 
     SubscriptBox["yd", "1"], "}"}], ",", 
    RowBox[{"{", 
     SubscriptBox["yd", "2"], "}"}], ",", 
    RowBox[{"{", 
     SubscriptBox["yd", "3"], "}"}], ",", 
    RowBox[{"{", 
     SubscriptBox["yd", "4"], "}"}], ",", 
    RowBox[{"{", 
     SubscriptBox["yd", "5"], "}"}]}], "}"}]}]], "Output",
 CellChangeTimes->{
  3.5427383843364487`*^9, 3.5427384415077105`*^9, {3.5429008148452897`*^9, 
   3.5429008399769335`*^9}, 3.5429010566457143`*^9, 3.5429021433861723`*^9, 
   3.5429046292650456`*^9, 3.542907084617917*^9, 3.542931220854525*^9, 
   3.5429313493308735`*^9, 3.5429853191565886`*^9, 3.54298750933286*^9, 
   3.542998285184204*^9, 3.5430000056594057`*^9, 3.5430107253384914`*^9, 
   3.543010906684864*^9, 3.5430794577487993`*^9, 3.543079655565398*^9, 
   3.5430803151601114`*^9, 3.5431062953380055`*^9, 3.5433194807550945`*^9, 
   3.543319980563682*^9, 3.5433638836530476`*^9, 3.5438619142351527`*^9, 
   3.545265897276168*^9, 3.545266678724337*^9, 3.545769061398631*^9, 
   3.5472214474559183`*^9, 3.5472285529833307`*^9, 3.547229432456634*^9, 
   3.547229887780677*^9, 3.5472299830631266`*^9, {3.5472308246512628`*^9, 
   3.547230840371162*^9}, 3.5472322609164124`*^9, 3.5472423160346622`*^9, 
   3.547309405475829*^9, 3.5474027957331686`*^9, 3.5474032570725555`*^9, 
   3.5474035297161503`*^9, 3.5474915573330417`*^9, 3.5477686422248306`*^9, 
   3.5478304601561747`*^9, 3.547899864412871*^9, 3.551541465670022*^9, 
   3.5526506422578344`*^9, 3.5526529849908314`*^9, 3.552828547552743*^9, 
   3.552932604286448*^9, 3.5563180404003067`*^9, 3.556325533299346*^9, 
   3.5563273372217703`*^9, 3.556385653987318*^9, 3.5568337078192787`*^9, 
   3.581337440281351*^9, 3.588020802217132*^9, 3.5926615969435096`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"temp", "=", 
  RowBox[{
   RowBox[{
    RowBox[{"yplus", "\[LeftDoubleBracket]", "3", "\[RightDoubleBracket]"}], 
    " ", "+", 
    RowBox[{"yplus", "\[LeftDoubleBracket]", "4", "\[RightDoubleBracket]"}]}],
    "//", "N"}]}]], "Input",
 CellChangeTimes->{{3.5427376306000757`*^9, 3.5427376561841965`*^9}, {
  3.5427377344241447`*^9, 3.5427377526966515`*^9}, {3.542737792792188*^9, 
  3.5427378048884525`*^9}, {3.542738019833596*^9, 3.5427380222806664`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{"-", "1.`"}], " ", 
    RowBox[{"a", "[", 
     RowBox[{"4.`", ",", "1.`"}], "]"}]}], "-", 
   RowBox[{"1.`", " ", 
    RowBox[{"a", "[", 
     RowBox[{"4.`", ",", "5.`"}], "]"}]}], "-", 
   RowBox[{"1.`", " ", 
    RowBox[{"a", "[", 
     RowBox[{"5.`", ",", "1.`"}], "]"}]}], "-", 
   RowBox[{"1.`", " ", 
    RowBox[{"a", "[", 
     RowBox[{"5.`", ",", "5.`"}], "]"}]}], "+", 
   RowBox[{"0.49921996879875197`", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.5427380086465592`*^9, 3.542738023259118*^9}, 
   3.542738442389741*^9, {3.542900821631302*^9, 3.5429008453121433`*^9}, 
   3.542901114506216*^9, 3.5429021434201746`*^9, 3.5429046292806454`*^9, 
   3.542907084633517*^9, 3.542931220874526*^9, 3.542931349359875*^9, 
   3.54298531917859*^9, 3.542987509351861*^9, 3.5429982871203146`*^9, 
   3.543000005675006*^9, 3.543010725363493*^9, 3.5430109067108655`*^9, 
   3.5430794577956*^9, 3.5430796555934*^9, 3.5430803151931133`*^9, 
   3.5431062953600073`*^9, 3.543319480767095*^9, 3.5433199805746827`*^9, 
   3.543363883664048*^9, 3.5438619142821555`*^9, 3.5452658973151703`*^9, 
   3.5452666787333374`*^9, 3.5457690614056315`*^9, 3.5472214475129213`*^9, 
   3.547228553076336*^9, 3.547229432527638*^9, 3.5472298877936773`*^9, 
   3.54722998312013*^9, {3.5472308246942654`*^9, 3.547230840391163*^9}, 
   3.5472322609694157`*^9, 3.547242316148669*^9, 3.547309405510831*^9, 
   3.5474027957831717`*^9, 3.547403257094557*^9, 3.547403529770153*^9, 
   3.5474915574030457`*^9, 3.547768642368839*^9, 3.547830460204177*^9, 
   3.547899864442872*^9, 3.551541465716822*^9, 3.552650642285836*^9, 
   3.552652985020833*^9, 3.552828547587745*^9, 3.5529326043024487`*^9, 
   3.5563180404573097`*^9, 3.5563255333623495`*^9, 3.5563273372747726`*^9, 
   3.5563856540413213`*^9, 3.556833707856805*^9, 3.5813374404283595`*^9, 
   3.588020802425144*^9, 3.5926615969755116`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"yplus", "\[LeftDoubleBracket]", "3", "\[RightDoubleBracket]"}], 
   "+", 
   RowBox[{"yplus", "\[LeftDoubleBracket]", "5", "\[RightDoubleBracket]"}], 
   "+", "temp"}], "//", "N"}]], "Input",
 CellChangeTimes->{{3.5427378562804327`*^9, 3.5427378665845976`*^9}, {
  3.5427379004085817`*^9, 3.542737901336232*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{"-", "2.`"}], " ", 
    RowBox[{"a", "[", 
     RowBox[{"4.`", ",", "1.`"}], "]"}]}], "-", 
   RowBox[{"2.`", " ", 
    RowBox[{"a", "[", 
     RowBox[{"4.`", ",", "5.`"}], "]"}]}], "-", 
   RowBox[{"1.`", " ", 
    RowBox[{"a", "[", 
     RowBox[{"5.`", ",", "1.`"}], "]"}]}], "-", 
   RowBox[{"1.`", " ", 
    RowBox[{"a", "[", 
     RowBox[{"5.`", ",", "5.`"}], "]"}]}], "-", 
   RowBox[{"1.`", " ", 
    RowBox[{"a", "[", 
     RowBox[{"6.`", ",", "1.`"}], "]"}]}], "-", 
   RowBox[{"1.`", " ", 
    RowBox[{"a", "[", 
     RowBox[{"6.`", ",", "5.`"}], "]"}]}], "-", 
   RowBox[{"1.`", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
   RowBox[{"0.0015600624024960652`", " ", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.542998299714035*^9, 3.5430000057062063`*^9, 3.543010725390495*^9, 
   3.543010906741867*^9, 3.5430794578423996`*^9, 3.543079655626402*^9, 
   3.5430803152301154`*^9, 3.5431062953840084`*^9, 3.543319480781096*^9, 
   3.5433199805876837`*^9, 3.5433638836770487`*^9, 3.5438619143231583`*^9, 
   3.545265897355172*^9, 3.5452666787463384`*^9, 3.5457690614156322`*^9, 
   3.547221447565924*^9, 3.5472285531693416`*^9, 3.547229432596642*^9, 
   3.5472298878086786`*^9, 3.547229983175133*^9, {3.5472308247732697`*^9, 
   3.5472308404151645`*^9}, 3.5472322610234184`*^9, 3.54724231634768*^9, 
   3.547309405542833*^9, 3.5474027958361745`*^9, 3.547403257118558*^9, 
   3.547403529822156*^9, 3.54749155747705*^9, 3.547768642403841*^9, 
   3.5478304602461796`*^9, 3.5478998644758744`*^9, 3.551541465748022*^9, 
   3.5526506423118377`*^9, 3.5526529850578346`*^9, 3.552828547619747*^9, 
   3.55293260432145*^9, 3.5563180405133133`*^9, 3.5563255334193525`*^9, 
   3.556327337325776*^9, 3.5563856540953245`*^9, 3.5568337078938713`*^9, 
   3.5813374405273647`*^9, 3.58802080253415*^9, 3.592661597000513*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yplus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515893576706831*^9, 3.51589357756488*^9}, 
   3.542550617721233*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"yminus", "=", 
   RowBox[{
    SubscriptBox["y", "2"], "/.", "resetmap"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158912072393045`*^9, 3.515891227677474*^9}, {
   3.515943128370984*^9, 3.515943134586339*^9}, {3.515943165683118*^9, 
   3.515943166187147*^9}, 3.5417803855632095`*^9, {3.5424727131428027`*^9, 
   3.5424727181348114`*^9}, {3.542550625792694*^9, 3.5425506534152746`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], 
  ";"}], "\[IndentingNewLine]"}], "Input",
 CellChangeTimes->{{3.5158935571417117`*^9, 3.515893586749405*^9}}],

Cell[CellGroupData[{

Cell["Velocity of Outputs: Pre and Post Impact", "Subsection",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.5133462845249467`*^9}, {
  3.5140475842599964`*^9, 3.514047587214165*^9}, {3.514050381757004*^9, 
  3.5140503867632904`*^9}, {3.5140508345048995`*^9, 3.514050836366006*^9}, {
  3.515340123141806*^9, 3.515340124962953*^9}}],

Cell["compute the jacobian of the output functions", "Text",
 CellChangeTimes->{{3.514056134624614*^9, 3.514056152522016*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Y", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"{", 
       SubscriptBox["deltap", "hip"], "}"}]}], ",", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", "2"], "]"}]}], "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158871007064247`*^9, 3.515887101970497*^9}, 
   3.5158874557787333`*^9, {3.515898875609911*^9, 3.5158988760079336`*^9}, {
   3.5158993719242983`*^9, 3.5158993862991204`*^9}, {3.515899472917075*^9, 
   3.5158994757032337`*^9}, {3.541868195655182*^9, 3.5418681967783833`*^9}, {
   3.5429018285643053`*^9, 3.5429018365827193`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Yplus", "=", 
  RowBox[{
   RowBox[{
    RowBox[{"Y", "/.", "initialtime"}], "/.", "constsubs"}], "/.", 
   "resetmap"}]}]], "Input",
 CellChangeTimes->{{3.5158905547469845`*^9, 3.5158905560910616`*^9}, 
   3.515891153092208*^9, {3.515893378403488*^9, 3.5158933836337876`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox["81407", "100000"]}], ",", 
     RowBox[{"-", 
      FractionBox["40767", "100000"]}], ",", "0", ",", "0", ",", "0", ",", 
     "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox[
      RowBox[{"81407", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "2"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "3"}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", 
     FractionBox[
      RowBox[{
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}]}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", "0", ",", "0", ",", "0", ",", 
     "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox[
      RowBox[{"81407", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "2"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "3"}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", 
     FractionBox[
      RowBox[{"40767", " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "2"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "3"}], "]"}]}], "-", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", "0", ",", "0", ",", "1", ",", 
     "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"-", "100000"}], " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}], "+", 
       RowBox[{"81407", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}]}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"-", "100000"}], " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}]}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", 
     RowBox[{"-", "1"}], ",", "1", ",", 
     FractionBox["320", "641"], ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}], "+", 
       RowBox[{"81407", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}]}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", 
     FractionBox[
      RowBox[{
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}]}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", "1", ",", "0", ",", "0", ",", 
     "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}], "+", 
       RowBox[{"81407", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "4"}], "]"}]}]}], ")"}]}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", 
     FractionBox[
      RowBox[{
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "2"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "3"}], "]"}]}], "-", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "1"}], "]"}], " ", 
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "4"}], "]"}]}]}], ")"}]}]}], 
      RowBox[{"100000", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], ",", "1", ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}]}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.525028894384092*^9, 3.525040959635159*^9, 3.525053978954082*^9, 
   3.525221830965542*^9, 3.525222138288193*^9, 3.525222755707529*^9, 
   3.5418651194281034`*^9, 3.541869156538869*^9, 3.5424897646453047`*^9, {
   3.542491147624407*^9, 3.5424911640903482`*^9}, 3.542494460877612*^9, 
   3.542542652235407*^9, 3.542543018695367*^9, 3.54255067887473*^9, 
   3.542556632518264*^9, 3.542557145202021*^9, 3.542731734259087*^9, 
   3.542731798999201*^9, 3.5427337015704317`*^9, 3.542734415512973*^9, 
   3.5427344629659977`*^9, 3.5427345904830647`*^9, 3.542734732400713*^9, 
   3.542734867243525*^9, 3.5427354277089195`*^9, 3.542736156267253*^9, 
   3.5427362608711624`*^9, 3.5427367970246286`*^9, 3.5427368620265656`*^9, {
   3.542900865436178*^9, 3.542900868431384*^9}, {3.542901816973485*^9, 
   3.5429018382675223`*^9}, 3.5429021445592394`*^9, 3.5429046303258476`*^9, 
   3.5429070856787186`*^9, 3.542931222079595*^9, 3.5429313505729446`*^9, 
   3.5429853204096603`*^9, 3.5429875105879316`*^9, 3.5429983121397457`*^9, 
   3.5430000070634084`*^9, 3.543010726685569*^9, 3.543010907992939*^9, 
   3.543079459368272*^9, 3.543079657079485*^9, 3.5430803167552023`*^9, 
   3.543106296584077*^9, 3.5433194819771643`*^9, 3.5433199817867517`*^9, 
   3.543363884886118*^9, 3.5438619159212494`*^9, 3.545265898578241*^9, 
   3.5452666798874035`*^9, 3.5457690625376964`*^9, 3.547221448817996*^9, 
   3.547228553280348*^9, 3.547229432773652*^9, 3.547229889106753*^9, 
   3.5472299844222045`*^9, {3.5472308260023403`*^9, 3.5472308416242332`*^9}, 
   3.5472322622694902`*^9, 3.5472423233199267`*^9, 3.5473094067459016`*^9, 
   3.5474027970142417`*^9, 3.5474032583136263`*^9, 3.547403530973222*^9, 
   3.5474915586981196`*^9, 3.5477686441149387`*^9, 3.5478304614262466`*^9, 
   3.5478998656219397`*^9, 3.551541466918024*^9, 3.5526506434469023`*^9, 
   3.552652986174899*^9, 3.5528285487598124`*^9, 3.552932605442514*^9, 
   3.5563180423004155`*^9, 3.556325535194454*^9, 3.556327338994871*^9, 
   3.556385655756419*^9, 3.556833709670965*^9, 3.5813374468187246`*^9, 
   3.58802080935454*^9, 3.5926615976545506`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"Yplus", "/.", "statesubs"}], "/.", "constsubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158948955412636`*^9, 3.5158949312793083`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Yminus", "=", 
  RowBox[{
   RowBox[{"Y", "/.", "constsubs"}], "/.", "resetmap"}]}]], "Input",
 CellChangeTimes->{{3.5158912072393045`*^9, 3.515891227677474*^9}, {
   3.5158934248751464`*^9, 3.515893427941322*^9}, 3.541780404924317*^9, {
   3.5418651387458215`*^9, 3.541865146551185*^9}, 3.5424902028483686`*^9, 
   3.5424911402139826`*^9, {3.5429008793826027`*^9, 3.542900883735011*^9}, 
   3.542901819313489*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox["81407", "100000"]}], ",", 
     RowBox[{"-", 
      FractionBox["40767", "100000"]}], ",", "0", ",", "0", ",", "0", ",", 
     "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"81407", " ", 
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"100000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     "0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"81407", " ", 
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"3", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{"40767", " ", 
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"3", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"3", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     "0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"-", "100000"}], " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"81407", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{"81407", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"4", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"-", "100000"}], " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"4", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{"-", "1"}], ",", "1", ",", 
     FractionBox["320", "641"], ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"100000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"81407", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{"81407", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"5", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"100000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"5", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     "1", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"100000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"81407", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{"81407", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"100000", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        RowBox[{"-", 
         FractionBox[
          RowBox[{"127", " ", 
           RowBox[{"a", "[", 
            RowBox[{"6", ",", "4"}], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", "641"}], " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
             RowBox[{"321", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
             RowBox[{"641", " ", 
              RowBox[{
               SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
          RowBox[{"100000", " ", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]}]]}]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"100000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}], "-", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{"127", " ", 
             RowBox[{"a", "[", 
              RowBox[{"6", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
                RowBox[{"-", "641"}], " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "+", 
               RowBox[{"321", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "+", 
               RowBox[{"641", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]}], ")"}]}], 
            
            RowBox[{"100000", " ", 
             RowBox[{"a", "[", 
              RowBox[{"1", ",", "1"}], "]"}]}]], "]"}]}]}], ")"}]}]}], ",", 
     "1", ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}]}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.54290181968789*^9, 3.5429018432127314`*^9}, 
   3.5429021446142426`*^9, 3.5429046303570476`*^9, 3.542907085694319*^9, 
   3.542931222120598*^9, 3.542931350615947*^9, 3.5429853204546633`*^9, 
   3.542987510621934*^9, 3.542998314522882*^9, 3.5430000071102085`*^9, 
   3.5430107267365713`*^9, 3.543010908032941*^9, 3.543079459435275*^9, 
   3.5430796571284876`*^9, 3.5430803168062057`*^9, 3.543106296622079*^9, 
   3.5433194820301676`*^9, 3.5433199818107533`*^9, 3.5433638849301205`*^9, 
   3.5438619159902534`*^9, 3.5452658986252437`*^9, 3.5452666799094048`*^9, 
   3.5457690625566974`*^9, 3.5472214488599987`*^9, 3.547228553333351*^9, 
   3.5472294328156548`*^9, 3.547229889161756*^9, 3.547229984491208*^9, {
   3.5472308260633435`*^9, 3.547230841690237*^9}, 3.547232262304492*^9, 
   3.5472423234999275`*^9, 3.547309406799905*^9, 3.547402797085246*^9, 
   3.5474032583496284`*^9, 3.5474035310372257`*^9, 3.5474915587871246`*^9, 
   3.547768644173942*^9, 3.5478304614852505`*^9, 3.5478998656739426`*^9, 
   3.5515414669648247`*^9, 3.552650643490905*^9, 3.5526529862159014`*^9, 
   3.552828548806815*^9, 3.552932605478516*^9, 3.556318042360419*^9, 
   3.5563255352824593`*^9, 3.556327339077876*^9, 3.556385655827423*^9, 
   3.556833709748479*^9, 3.5813374471297426`*^9, 3.5880208096555576`*^9, 
   3.592661597681552*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515895085079105*^9, 3.5158950853751216`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{"h", "\[IndentingNewLine]", 
 RowBox[{"Dh", " ", "=", " ", 
  RowBox[{
   SubscriptBox["\[PartialD]", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
   RowBox[{"{", "h", "}"}]}]}]}], "Input",
 CellChangeTimes->{{3.5158876222012525`*^9, 3.5158876466726522`*^9}, {
  3.515895637521703*^9, 3.5158956394738145`*^9}}],

Cell[BoxData[
 RowBox[{
  FractionBox["1", "100000"], 
  RowBox[{"(", 
   RowBox[{"4251", "+", 
    RowBox[{"40640", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
    RowBox[{"40767", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
    RowBox[{"40767", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
    RowBox[{"40640", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
    RowBox[{"4251", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], 
   ")"}]}]], "Output",
 CellChangeTimes->{
  3.5424944609406157`*^9, 3.542542652306411*^9, 3.5425430187613707`*^9, {
   3.5425506908134136`*^9, 3.5425506953236713`*^9}, 3.5425566325882635`*^9, 
   3.542557145262021*^9, 3.5427317373166924`*^9, 3.542731799077201*^9, 
   3.5427337016364355`*^9, 3.5427344155929775`*^9, 3.542734463062003*^9, 
   3.5427345905870705`*^9, 3.542734732442715*^9, 3.542734867363532*^9, 
   3.542735427792924*^9, 3.542736156361258*^9, 3.542736260971168*^9, 
   3.542736797122634*^9, 3.5427368621255713`*^9, 3.542902144671246*^9, 
   3.542904630419448*^9, 3.542907085741119*^9, 3.5429312221706004`*^9, 
   3.5429313506679497`*^9, 3.5429853205006657`*^9, 3.542987510665936*^9, 
   3.5429983191271453`*^9, 3.543000007157009*^9, 3.5430107267945747`*^9, 
   3.543010908080944*^9, 3.54307945952028*^9, 3.5430796571894913`*^9, 
   3.543080316867209*^9, 3.5431062966690817`*^9, 3.54331948206917*^9, 
   3.5433199818437552`*^9, 3.5433638849821234`*^9, 3.5438619160972595`*^9, 
   3.5452658986832466`*^9, 3.545266679940407*^9, 3.545769062585699*^9, 
   3.5472214489070005`*^9, 3.5472285533883543`*^9, 3.547229432868657*^9, 
   3.5472298892277594`*^9, 3.547229984572213*^9, {3.547230826134348*^9, 
   3.5472308417652416`*^9}, 3.5472322623504944`*^9, 3.5472423238629465`*^9, 
   3.547309406861908*^9, 3.5474027971672506`*^9, 3.547403258388631*^9, 
   3.54740353111523*^9, 3.54749155887813*^9, 3.547768644248946*^9, 
   3.547830461548254*^9, 3.5478998657289457`*^9, 3.5515414670272245`*^9, 
   3.5526506435379076`*^9, 3.552652986266904*^9, 3.552828548859818*^9, 
   3.552932605517518*^9, 3.5563180424434233`*^9, 3.556325535395466*^9, 
   3.5563273391718817`*^9, 3.556385655929429*^9, 3.55683370985216*^9, 
   3.581337447234749*^9, 3.5880208097955656`*^9, 3.592661597717554*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     FractionBox["1", "100000"], 
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        RowBox[{"-", "40640"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"40767", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"40767", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"40640", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"4251", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], 
      ")"}]}], ",", 
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{"-", "40767"}], " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"40767", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"40640", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"4251", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], 
     "100000"], ",", 
    FractionBox[
     RowBox[{
      RowBox[{"40767", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"40640", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"4251", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], 
     "100000"], ",", 
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{"-", "40767"}], " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
      RowBox[{"40640", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
      RowBox[{"4251", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], 
     "100000"], ",", 
    FractionBox[
     RowBox[{
      RowBox[{
       RowBox[{"-", "40640"}], " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
      RowBox[{"4251", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], 
     "100000"], ",", 
    RowBox[{"-", 
     FractionBox[
      RowBox[{"4251", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}], 
      "100000"]}]}], "}"}], "}"}]], "Output",
 CellChangeTimes->{
  3.5424944609406157`*^9, 3.542542652306411*^9, 3.5425430187613707`*^9, {
   3.5425506908134136`*^9, 3.5425506953236713`*^9}, 3.5425566325882635`*^9, 
   3.542557145262021*^9, 3.5427317373166924`*^9, 3.542731799077201*^9, 
   3.5427337016364355`*^9, 3.5427344155929775`*^9, 3.542734463062003*^9, 
   3.5427345905870705`*^9, 3.542734732442715*^9, 3.542734867363532*^9, 
   3.542735427792924*^9, 3.542736156361258*^9, 3.542736260971168*^9, 
   3.542736797122634*^9, 3.5427368621255713`*^9, 3.542902144671246*^9, 
   3.542904630419448*^9, 3.542907085741119*^9, 3.5429312221706004`*^9, 
   3.5429313506679497`*^9, 3.5429853205006657`*^9, 3.542987510665936*^9, 
   3.5429983191271453`*^9, 3.543000007157009*^9, 3.5430107267945747`*^9, 
   3.543010908080944*^9, 3.54307945952028*^9, 3.5430796571894913`*^9, 
   3.543080316867209*^9, 3.5431062966690817`*^9, 3.54331948206917*^9, 
   3.5433199818437552`*^9, 3.5433638849821234`*^9, 3.5438619160972595`*^9, 
   3.5452658986832466`*^9, 3.545266679940407*^9, 3.545769062585699*^9, 
   3.5472214489070005`*^9, 3.5472285533883543`*^9, 3.547229432868657*^9, 
   3.5472298892277594`*^9, 3.547229984572213*^9, {3.547230826134348*^9, 
   3.5472308417652416`*^9}, 3.5472322623504944`*^9, 3.5472423238629465`*^9, 
   3.547309406861908*^9, 3.5474027971672506`*^9, 3.547403258388631*^9, 
   3.54740353111523*^9, 3.54749155887813*^9, 3.547768644248946*^9, 
   3.547830461548254*^9, 3.5478998657289457`*^9, 3.5515414670272245`*^9, 
   3.5526506435379076`*^9, 3.552652986266904*^9, 3.552828548859818*^9, 
   3.552932605517518*^9, 3.5563180424434233`*^9, 3.556325535395466*^9, 
   3.5563273391718817`*^9, 3.556385655929429*^9, 3.55683370985216*^9, 
   3.581337447234749*^9, 3.5880208097955656`*^9, 3.592661597718554*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_minus\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Dh", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158874776569853`*^9, 3.5158874849574027`*^9}, {
  3.515887517663273*^9, 3.5158875400105515`*^9}, {3.515887575902604*^9, 
  3.5158875781407323`*^9}, {3.515895688195601*^9, 3.515895688494618*^9}, {
  3.51589577315246*^9, 3.515895776011624*^9}, {3.515895808711494*^9, 
  3.515895808919506*^9}, {3.5158958389342227`*^9, 3.515895841410364*^9}}],

Cell[CellGroupData[{

Cell["solve for the inverse kinematics", "Subsubsection",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.5133462845249467`*^9}, {
  3.5140475842599964`*^9, 3.514047587214165*^9}, {3.514050381757004*^9, 
  3.5140503867632904`*^9}, {3.5140508768053193`*^9, 3.5140508825446477`*^9}, {
  3.514056179159212*^9, 3.51405618309467*^9}}],

Cell["create a function to solve the inverse kinematics", "Text",
 CellChangeTimes->{{3.514058861590675*^9, 3.514058871479416*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"SolveEquations", "[", 
    RowBox[{"equations_", ",", "variables_", ",", "solutionnumbers_"}], "]"}],
    ":=", "\[IndentingNewLine]", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"i", ",", 
       RowBox[{"solutions", "=", 
        RowBox[{"{", "}"}]}], ",", "tmpeqn", ",", "tmpsolution"}], "}"}], ",",
      "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"For", "[", 
       RowBox[{
        RowBox[{"i", "=", "1"}], ",", 
        RowBox[{"i", "\[LessEqual]", 
         RowBox[{"Length", "[", "variables", "]"}]}], ",", 
        RowBox[{"i", "++"}], ",", "\[IndentingNewLine]", 
        RowBox[{
         RowBox[{"tmpeqn", "=", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"equations", "[", 
             RowBox[{"[", "i", "]"}], "]"}], ")"}], "\[Equal]", "0"}]}], ";", 
         "\[IndentingNewLine]", 
         RowBox[{"tmpsolution", "=", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
            "variables", "\[LeftDoubleBracket]", "i", 
             "\[RightDoubleBracket]"}], "\[Rule]", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
              "variables", "\[LeftDoubleBracket]", "i", 
               "\[RightDoubleBracket]"}], "/.", 
              RowBox[{"Flatten", "[", 
               RowBox[{"{", 
                RowBox[{"solutions", ",", 
                 RowBox[{
                  RowBox[{"Solve", "[", 
                   RowBox[{"tmpeqn", ",", 
                    RowBox[{
                    "variables", "\[LeftDoubleBracket]", "i", 
                    "\[RightDoubleBracket]"}]}], "]"}], 
                  "\[LeftDoubleBracket]", 
                  RowBox[{
                  "solutionnumbers", "\[LeftDoubleBracket]", "i", 
                   "\[RightDoubleBracket]"}], "\[RightDoubleBracket]"}]}], 
                "}"}], "]"}]}], ")"}]}], "}"}]}], ";", "\[IndentingNewLine]", 
         
         RowBox[{"solutions", "=", 
          RowBox[{"Join", "[", 
           RowBox[{"solutions", ",", 
            RowBox[{"(", 
             RowBox[{"tmpsolution", "/.", "solutions"}], ")"}]}], "]"}]}]}]}],
        "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", 
      "solutions"}]}], "\[IndentingNewLine]", "]"}]}], ";"}]], "Input"],

Cell[BoxData[
 RowBox[{
  RowBox[{"atest", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "1"}], "]"}], "\[Rule]", "0.8300"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "2"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "3"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "4"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "5"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.2024"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "2"}], "]"}], "\[Rule]", "13.5080"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "3"}], "]"}], "\[Rule]", "0.0470"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "4"}], "]"}], "\[Rule]", "3.8943"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "5"}], "]"}], "\[Rule]", "0.3315"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.3452"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "2"}], "]"}], "\[Rule]", "10.7909"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "3"}], "]"}], "\[Rule]", "0.0133"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "4"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.8249"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "5"}], "]"}], "\[Rule]", "0.6888"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "1"}], "]"}], "\[Rule]", "0.1395"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "2"}], "]"}], "\[Rule]", "7.2676"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "3"}], "]"}], "\[Rule]", "0.1619"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "4"}], "]"}], "\[Rule]", 
      RowBox[{"-", "1.4881"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}], "\[Rule]", "0.1713"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.0144"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "2"}], "]"}], "\[Rule]", "0.0069"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "3"}], "]"}], "\[Rule]", 
      RowBox[{"-", "142.8555"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "4"}], "]"}], "\[Rule]", "32.7943"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "5"}], "]"}], "\[Rule]", "0.0773"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"6", ",", "1"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"6", ",", "2"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"6", ",", "3"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"6", ",", "4"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"6", ",", "5"}], "]"}], "\[Rule]", "0"}]}], "}"}]}], 
  ";"}]], "Input",
 CellChangeTimes->{{3.516708534127338*^9, 3.516708546737059*^9}, {
   3.5167085869383583`*^9, 3.516708631586912*^9}, {3.541817704366083*^9, 
   3.5418177151587005`*^9}, {3.541818606087659*^9, 3.5418186363343887`*^9}, 
   3.5424844318007464`*^9, {3.542999503138522*^9, 3.542999729307719*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"A", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"i", ",", "j"}], "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "6"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"j", ",", "5"}], "}"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.516708550491274*^9, 3.516708565841152*^9}, {
   3.5219868472809534`*^9, 3.5219868535253105`*^9}, {3.5418177246822453`*^9, 
   3.541817751080755*^9}, {3.5418187621085825`*^9, 3.5418187628926277`*^9}, 
   3.5424844363871546`*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"A", "/.", "atest"}]], "Input",
 CellChangeTimes->{{3.5167085746056533`*^9, 3.5167085760477357`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"0.83`", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "0.2024`"}], ",", "13.508`", ",", "0.047`", ",", "3.8943`", 
     ",", "0.3315`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "0.3452`"}], ",", "10.7909`", ",", "0.0133`", ",", 
     RowBox[{"-", "0.8249`"}], ",", "0.6888`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"0.1395`", ",", "7.2676`", ",", "0.1619`", ",", 
     RowBox[{"-", "1.4881`"}], ",", "0.1713`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "0.0144`"}], ",", "0.0069`", ",", 
     RowBox[{"-", "142.8555`"}], ",", "32.7943`", ",", "0.0773`"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.525028890483443*^9, 3.525040955835466*^9, 3.525053975237398*^9, 
   3.52522182743584*^9, 3.525222135077095*^9, 3.525222752568503*^9, {
   3.541817728169445*^9, 3.5418177541999335`*^9}, 3.541819518034819*^9, 
   3.541863784396304*^9, 3.5418682491008754`*^9, 3.541870798141819*^9, 
   3.542471648444461*^9, 3.5424800337095013`*^9, {3.5424844398347607`*^9, 
   3.542484441316763*^9}, 3.5425424587863417`*^9, 3.5429980576531897`*^9, 
   3.542998328954707*^9, 3.5429997387145357`*^9, 3.5430000071882086`*^9, 
   3.543010726844578*^9, 3.5430109081299467`*^9, 3.543079459592284*^9, 
   3.5430796572454944`*^9, 3.5430803169272127`*^9, 3.5431062967210846`*^9, 
   3.5433194821001716`*^9, 3.5433199818667564`*^9, 3.5433638850221257`*^9, 
   3.5438619161702633`*^9, 3.5452658987262497`*^9, 3.5452666799614077`*^9, 
   3.5457690626057005`*^9, 3.5472214489520035`*^9, 3.547228553455358*^9, 
   3.547229432928661*^9, 3.547229889287763*^9, 3.547229984655218*^9, {
   3.5472308262013516`*^9, 3.5472308418502464`*^9}, 3.5472322624034977`*^9, 
   3.547242324464981*^9, 3.547309406915911*^9, 3.547402797241255*^9, 
   3.5474032584356337`*^9, 3.547403531187234*^9, 3.547491558963135*^9, 
   3.54776864431995*^9, 3.5478304616102576`*^9, 3.5478998657799487`*^9, 
   3.5515414670896244`*^9, 3.5526506435799103`*^9, 3.5526529863109064`*^9, 
   3.552828548917821*^9, 3.5529326055515203`*^9, 3.5563180425244284`*^9, 
   3.556325535504472*^9, 3.5563273392598867`*^9, 3.5563856560394354`*^9, 
   3.556833709968526*^9, 3.5813374473147535`*^9, 3.5880208099165726`*^9, 
   3.5926615977535563`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData["yplus"], "Input",
 CellChangeTimes->{{3.5424728683928757`*^9, 3.5424728694692783`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     FractionBox[
      RowBox[{"320", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "641"], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"6", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"6", ",", "5"}], "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.542472870108879*^9, 3.5424800337715054`*^9, {3.542484443563167*^9, 
   3.5424844606919975`*^9}, 3.5425424588173437`*^9, 3.542998060240338*^9, 
   3.5429983315958586`*^9, 3.543000007203809*^9, 3.5430107268655787`*^9, 
   3.543010908151948*^9, 3.5430794596192856`*^9, 3.543079657270496*^9, 
   3.543080316955214*^9, 3.543106296743086*^9, 3.543319482107172*^9, 
   3.543319981872757*^9, 3.5433638850371265`*^9, 3.5438619162092657`*^9, 
   3.5452658987422504`*^9, 3.545266679966408*^9, 3.5457690626087008`*^9, 
   3.5472214489690046`*^9, 3.5472285535033607`*^9, 3.547229432963663*^9, 
   3.547229889321765*^9, 3.5472299847062206`*^9, {3.547230826241354*^9, 
   3.54723084191325*^9}, 3.5472322624214983`*^9, 3.547242324599989*^9, 
   3.5473094069409127`*^9, 3.5474027972862573`*^9, 3.5474032584516344`*^9, 
   3.547403531233237*^9, 3.5474915590281386`*^9, 3.5477686443449516`*^9, 
   3.5478304616462593`*^9, 3.5478998658079505`*^9, 3.551541467120825*^9, 
   3.5526506435989113`*^9, 3.552652986335908*^9, 3.552828548944823*^9, 
   3.552932605562521*^9, 3.5563180425504293`*^9, 3.556325535562475*^9, 
   3.556327339304889*^9, 3.5563856560994387`*^9, 3.556833710007662*^9, 
   3.5813374473527555`*^9, 3.588020809977576*^9, 3.592661597784558*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus1", "=", 
  RowBox[{
  "yplus", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}]], "Input",\

 CellChangeTimes->{{3.5429986690971622`*^9, 3.542998699058876*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    RowBox[{"a", "[", 
     RowBox[{"2", ",", "1"}], "]"}]}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"2", ",", "5"}], "]"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.5429987009309835`*^9, 3.543000007219409*^9, 3.5430107268935804`*^9, 
   3.5430109081729493`*^9, 3.5430794596462874`*^9, 3.5430796572944975`*^9, 
   3.5430803169842157`*^9, 3.543106296764087*^9, 3.5433194821151724`*^9, 
   3.5433199818797574`*^9, 3.5433638850531273`*^9, 3.543861916245268*^9, 
   3.545265898758251*^9, 3.5452666799724083`*^9, 3.545769062613701*^9, 
   3.547221448986006*^9, 3.547228553549363*^9, 3.547229432999665*^9, 
   3.5472298893557673`*^9, 3.547229984759224*^9, {3.547230826283356*^9, 
   3.5472308419712534`*^9}, 3.5472322624395*^9, 3.547242324804001*^9, 
   3.547309406966914*^9, 3.54740279733226*^9, 3.5474032584686356`*^9, 
   3.547403531279239*^9, 3.547491559091142*^9, 3.5477686443729534`*^9, 
   3.5478304616802616`*^9, 3.547899865836952*^9, 3.5515414671520247`*^9, 
   3.5526506436189127`*^9, 3.55265298636191*^9, 3.5528285489738245`*^9, 
   3.5529326055735216`*^9, 3.5563180426084332`*^9, 3.556325535623479*^9, 
   3.5563273393508916`*^9, 3.556385656154442*^9, 3.556833710048888*^9, 
   3.5813374474617615`*^9, 3.588020809998577*^9, 3.592661597800559*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus2", "=", 
  RowBox[{
  "yplus", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}]}]], "Input",\

 CellChangeTimes->{{3.5429986932585444`*^9, 3.5429987050102167`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    RowBox[{"a", "[", 
     RowBox[{"3", ",", "1"}], "]"}]}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"3", ",", "5"}], "]"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.542998706770317*^9, 3.5430000072350087`*^9, 3.543010726917582*^9, 
   3.54301090819495*^9, 3.5430794596742887`*^9, 3.543079657317498*^9, 
   3.5430803170122175`*^9, 3.5431062967850885`*^9, 3.543319482122173*^9, 
   3.543319981886758*^9, 3.543363885067128*^9, 3.5438619162792697`*^9, 
   3.545265898774252*^9, 3.545266679977409*^9, 3.545769062616701*^9, 
   3.5472214490020065`*^9, 3.547228553596366*^9, 3.5472294330326667`*^9, 
   3.547229889390769*^9, 3.547229984810227*^9, {3.5472308263233585`*^9, 
   3.547230842020256*^9}, 3.547232262458501*^9, 3.547242324950009*^9, 
   3.5473094069939156`*^9, 3.547402797377262*^9, 3.5474032584856367`*^9, 
   3.547403531326242*^9, 3.5474915591541457`*^9, 3.547768644397955*^9, 
   3.5478304617142634`*^9, 3.5478998658629537`*^9, 3.5515414671832247`*^9, 
   3.552650643637913*^9, 3.552652986386911*^9, 3.552828548998826*^9, 
   3.5529326055845222`*^9, 3.5563180426434355`*^9, 3.5563255356774817`*^9, 
   3.556327339395894*^9, 3.556385656214445*^9, 3.556833710083206*^9, 
   3.5813374474877634`*^9, 3.5880208100215783`*^9, 3.5926615978425612`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus3", " ", "=", " ", 
  RowBox[{
   RowBox[{"yplus", "\[LeftDoubleBracket]", "4", "\[RightDoubleBracket]"}], 
   " ", "+", " ", 
   RowBox[{
   "yplus", "\[LeftDoubleBracket]", "3", 
    "\[RightDoubleBracket]"}]}]}]], "Input",
 CellChangeTimes->{{3.5418190335931106`*^9, 3.541819090853386*^9}, {
   3.541819522115052*^9, 3.541819548177543*^9}, {3.542472851716447*^9, 
   3.542472856630455*^9}, {3.542472891855317*^9, 3.5424728919645176`*^9}, {
   3.542998335324072*^9, 3.54299834841082*^9}, 3.542998716841893*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    RowBox[{"a", "[", 
     RowBox[{"4", ",", "1"}], "]"}]}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"4", ",", "5"}], "]"}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"5", ",", "1"}], "]"}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"5", ",", "5"}], "]"}], "+", 
   FractionBox[
    RowBox[{"320", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "641"], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.5424728521376476`*^9, 3.5424728573636565`*^9}, 
   3.542472892713319*^9, 3.542480033815508*^9, 3.542484466838408*^9, 
   3.5425424588513455`*^9, {3.542998337196179*^9, 3.5429983489338503`*^9}, 
   3.542998717515932*^9, 3.5430000072662086`*^9, 3.543010726940583*^9, 
   3.5430109082169514`*^9, 3.5430794597002907`*^9, 3.5430796573415003`*^9, 
   3.5430803170412188`*^9, 3.54310629680709*^9, 3.543319482130173*^9, 
   3.543319981894758*^9, 3.543363885083129*^9, 3.543861916316272*^9, 
   3.5452658987902527`*^9, 3.5452666799844093`*^9, 3.5457690626217012`*^9, 
   3.5472214490210075`*^9, 3.5472285536453686`*^9, 3.5472294330756693`*^9, 
   3.547229889424771*^9, 3.54722998486123*^9, {3.547230826363361*^9, 
   3.547230842070259*^9}, 3.5472322624755015`*^9, 3.547242325072016*^9, 
   3.5473094070199175`*^9, 3.5474027974232655`*^9, 3.5474032585016375`*^9, 
   3.5474035313692446`*^9, 3.5474915592191496`*^9, 3.5477686444239564`*^9, 
   3.5478304617492657`*^9, 3.547899865888955*^9, 3.551541467214425*^9, 
   3.552650643657914*^9, 3.5526529864139123`*^9, 3.552828549024827*^9, 
   3.552932605595523*^9, 3.556318042676437*^9, 3.556325535730485*^9, 
   3.5563273394378967`*^9, 3.556385656271449*^9, 3.556833710116723*^9, 
   3.5813374475297656`*^9, 3.5880208100925827`*^9, 3.5926615978745627`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus4", "=", 
  RowBox[{
  "yplus", "\[LeftDoubleBracket]", "4", "\[RightDoubleBracket]"}]}]], "Input",\

 CellChangeTimes->{{3.5429985217767363`*^9, 3.542998534673474*^9}, {
  3.542998722826236*^9, 3.542998735305949*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    RowBox[{"a", "[", 
     RowBox[{"5", ",", "1"}], "]"}]}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"5", ",", "5"}], "]"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "+", 
   RowBox[{
    SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.5429985350104933`*^9, 3.542998736564021*^9, 3.543000007281809*^9, 
   3.5430107269655848`*^9, 3.543010908238953*^9, 3.543079459727292*^9, 
   3.5430796573655014`*^9, 3.543080317070221*^9, 3.5431062968280907`*^9, 
   3.543319482138174*^9, 3.5433199819017587`*^9, 3.54336388509813*^9, 
   3.543861916352274*^9, 3.545265898806254*^9, 3.54526667999041*^9, 
   3.5457690626267014`*^9, 3.5472214490360084`*^9, 3.5472285536923714`*^9, 
   3.547229433109671*^9, 3.5472298894587727`*^9, 3.5472299849142323`*^9, {
   3.547230826404363*^9, 3.547230842122262*^9}, 3.547232262491503*^9, 
   3.5472423252030234`*^9, 3.547309407046919*^9, 3.5474027974682674`*^9, 
   3.5474032585186386`*^9, 3.547403531413247*^9, 3.547491559284153*^9, 
   3.5477686444479575`*^9, 3.547830461782267*^9, 3.5478998659159565`*^9, 
   3.5515414672456245`*^9, 3.552650643680916*^9, 3.5526529864389143`*^9, 
   3.552828549055829*^9, 3.5529326056065235`*^9, 3.556318042712439*^9, 
   3.5563255357894883`*^9, 3.556327339480899*^9, 3.556385656328452*^9, 
   3.556833710148775*^9, 3.5813374476187706`*^9, 3.5880208101155834`*^9, 
   3.592661597890564*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus5", " ", "=", " ", 
  RowBox[{
   RowBox[{"yplus", "\[LeftDoubleBracket]", "3", "\[RightDoubleBracket]"}], 
   " ", "+", " ", 
   RowBox[{
   "yplus", "\[LeftDoubleBracket]", "5", 
    "\[RightDoubleBracket]"}]}]}]], "Input",
 CellChangeTimes->{{3.542998359685465*^9, 3.542998386585004*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    RowBox[{"a", "[", 
     RowBox[{"4", ",", "1"}], "]"}]}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"4", ",", "5"}], "]"}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"6", ",", "1"}], "]"}], "-", 
   RowBox[{"a", "[", 
    RowBox[{"6", ",", "5"}], "]"}], "-", 
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
   FractionBox[
    RowBox[{"321", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "641"]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.542998388553116*^9, 3.543000007297409*^9, 3.5430107269905863`*^9, 
   3.5430109082629547`*^9, 3.5430794597542934`*^9, 3.5430796573895025`*^9, 
   3.5430803170972223`*^9, 3.543106296849092*^9, 3.5433194821461744`*^9, 
   3.5433199819097595`*^9, 3.543363885114131*^9, 3.543861916390276*^9, 
   3.5452658988322554`*^9, 3.54526667999741*^9, 3.545769062632702*^9, 
   3.547221449053009*^9, 3.5472285537423744`*^9, 3.5472294331536736`*^9, 
   3.547229889494775*^9, 3.5472299849682355`*^9, {3.5472308264443655`*^9, 
   3.5472308421712646`*^9}, 3.5472322625095034`*^9, 3.5472423253420315`*^9, 
   3.5473094070729203`*^9, 3.54740279751527*^9, 3.5474032585356393`*^9, 
   3.5474035314572496`*^9, 3.547491559352157*^9, 3.547768644473959*^9, 
   3.547830461816269*^9, 3.5478998659419584`*^9, 3.551541467276825*^9, 
   3.552650643700917*^9, 3.5526529864649153`*^9, 3.5528285490818305`*^9, 
   3.552932605617524*^9, 3.5563180427504416`*^9, 3.5563255358454914`*^9, 
   3.556327339523902*^9, 3.5563856563864555`*^9, 3.556833710187796*^9, 
   3.5813374477087755`*^9, 3.588020810175587*^9, 3.5926615979215655`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus6", "=", 
  RowBox[{"{", "h", "}"}]}]], "Input",
 CellChangeTimes->{{3.542998758454273*^9, 3.542998772495076*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   FractionBox["1", "100000"], 
   RowBox[{"(", 
    RowBox[{"4251", "+", 
     RowBox[{"40640", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
     RowBox[{"40767", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
     RowBox[{"40767", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
     RowBox[{"40640", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
     RowBox[{"4251", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], ")"}]}],
   "}"}]], "Output",
 CellChangeTimes->{{3.542998764051594*^9, 3.5429987730221066`*^9}, 
   3.5430000073130093`*^9, 3.543010727024588*^9, 3.5430109082939563`*^9, 
   3.5430794597818947`*^9, 3.543079657423505*^9, 3.5430803171352243`*^9, 
   3.543106296883094*^9, 3.5433194821661754`*^9, 3.5433199819287605`*^9, 
   3.5433638851411324`*^9, 3.543861916440279*^9, 3.5452658988772583`*^9, 
   3.545266680014411*^9, 3.545769062649703*^9, 3.547221449080011*^9, 
   3.5472285537903767`*^9, 3.547229433198676*^9, 3.5472298895407777`*^9, 
   3.547229985027239*^9, {3.5472308264943686`*^9, 3.5472308422322683`*^9}, 
   3.547232262537505*^9, 3.5472423255220413`*^9, 3.5473094071099224`*^9, 
   3.547402797571274*^9, 3.547403258559641*^9, 3.5474035315132527`*^9, 
   3.547491559427161*^9, 3.5477686445179615`*^9, 3.547830461861272*^9, 
   3.5478998659779606`*^9, 3.551541467323625*^9, 3.552650643730919*^9, 
   3.5526529865009174`*^9, 3.5528285491178327`*^9, 3.5529326056395254`*^9, 
   3.5563180427854433`*^9, 3.5563255358994946`*^9, 3.556327339566904*^9, 
   3.5563856564444585`*^9, 3.556833710216069*^9, 3.5813374478007812`*^9, 
   3.5880208102415905`*^9, 3.5926615979395666`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"yhsolve", " ", "=", " ", 
   RowBox[{"Join", "[", 
    RowBox[{
    "yplus1", ",", "yplus2", ",", "yplus3", ",", "yplus4", ",", "yplus5", ",",
      "yplus6"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{"MatrixForm", "[", "yhsolve", "]"}]}], "Input",
 CellChangeTimes->{{3.5159453782986727`*^9, 3.515945470066921*^9}, {
   3.515946921330929*^9, 3.515946925987195*^9}, 3.5159579961003695`*^9, {
   3.5418180797945566`*^9, 3.541818082385705*^9}, {3.5418195576910872`*^9, 
   3.54181961875058*^9}, 3.5418196983721337`*^9, {3.541819731826047*^9, 
   3.541819739577491*^9}, 3.5418197744574857`*^9, {3.5418638023519354`*^9, 
   3.5418638645960445`*^9}, {3.5424729562678304`*^9, 3.54247296185264*^9}, {
   3.5429984033129606`*^9, 3.5429984920360355`*^9}, {3.5429986025203543`*^9, 
   3.5429986313910055`*^9}, {3.5429987905351086`*^9, 3.5429989592877607`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", 
   TagBox[GridBox[{
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"4", ",", "5"}], "]"}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"5", ",", "1"}], "]"}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"5", ",", "5"}], "]"}], "+", 
        FractionBox[
         RowBox[{"320", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "641"], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"5", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"4", ",", "5"}], "]"}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"6", ",", "1"}], "]"}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"6", ",", "5"}], "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
        FractionBox[
         RowBox[{"321", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "641"]}]},
      {
       FractionBox[
        RowBox[{"4251", "+", 
         RowBox[{"40640", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"40767", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"40767", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"40640", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "-", 
         RowBox[{"4251", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
            RowBox[{
             SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
            RowBox[{
             SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}]}]}], 
        "100000"]}
     },
     GridBoxAlignment->{
      "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
        "RowsIndexed" -> {}},
     GridBoxSpacings->{"Columns" -> {
         Offset[0.27999999999999997`], {
          Offset[0.5599999999999999]}, 
         Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
         Offset[0.2], {
          Offset[0.4]}, 
         Offset[0.2]}, "RowsIndexed" -> {}}],
    Column], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{{3.5429989140831747`*^9, 3.542998960136809*^9}, 
   3.543000007359809*^9, 3.5430107270665903`*^9, 3.5430109083409586`*^9, 
   3.5430794598442945`*^9, 3.5430796574745073`*^9, 3.5430803171902275`*^9, 
   3.5431062969300966`*^9, 3.543319482200177*^9, 3.543319981960762*^9, 
   3.5433638851821346`*^9, 3.543861916505283*^9, 3.5452658989352612`*^9, 
   3.5452666800444126`*^9, 3.5457690626777043`*^9, 3.547221449120013*^9, 
   3.5472285540103893`*^9, 3.547229433274681*^9, 3.5472298895987806`*^9, 
   3.547229985101243*^9, {3.5472308265563717`*^9, 3.5472308423102727`*^9}, 
   3.5472322625805073`*^9, 3.5472423256920514`*^9, 3.547309407160925*^9, 
   3.5474027976412773`*^9, 3.547403258602643*^9, 3.5474035315782566`*^9, 
   3.5474915595151663`*^9, 3.5477686445779653`*^9, 3.5478304619192753`*^9, 
   3.547899866026963*^9, 3.5515414673860254`*^9, 3.552650643773921*^9, 
   3.55265298654892*^9, 3.552828549164835*^9, 3.552932605675527*^9, 
   3.556318042819445*^9, 3.5563255359664984`*^9, 3.556327339613907*^9, 
   3.556385656551465*^9, 3.556833710255913*^9, 3.5813374479297886`*^9, 
   3.5880208104196014`*^9, 3.5926615979625683`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"Theta5Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta2Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta3Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "3", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta1Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "5", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta6Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "4", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"yhsolve", "\[LeftDoubleBracket]", "6", "\[RightDoubleBracket]"}], 
  "=", 
  RowBox[{
   RowBox[{"yhsolve", "\[LeftDoubleBracket]", "6", "\[RightDoubleBracket]"}], 
   "/.", "Theta6Cond"}]}]}], "Input",
 CellChangeTimes->{{3.51594676032572*^9, 3.5159468099305573`*^9}, {
   3.5159566741957607`*^9, 3.51595678713122*^9}, {3.5159568457875752`*^9, 
   3.5159568538670373`*^9}, {3.515956895125397*^9, 3.5159569150995398`*^9}, {
   3.5159571238684807`*^9, 3.515957170348139*^9}, {3.515958003729806*^9, 
   3.515958007675031*^9}, {3.5159638958349743`*^9, 3.515963896592017*^9}, {
   3.51596399752379*^9, 3.51596400259408*^9}, {3.515964082366643*^9, 
   3.515964102957821*^9}, {3.5159641483604174`*^9, 3.515964231857193*^9}, {
   3.515964279459916*^9, 3.5159642822650766`*^9}, {3.5159728721103873`*^9, 
   3.5159729359940414`*^9}, {3.515973121749666*^9, 3.5159731262559233`*^9}, 
   3.516711531312767*^9, {3.52072837720152*^9, 3.520728378225579*^9}, {
   3.522511106163158*^9, 3.5225111066501856`*^9}, {3.522511147690533*^9, 
   3.5225111497466507`*^9}, {3.5225118207090273`*^9, 3.522511820941041*^9}, {
   3.5418183907673435`*^9, 3.5418185753739023`*^9}, {3.5418186470990047`*^9, 
   3.5418186530663457`*^9}, {3.541818773371227*^9, 3.541818775619355*^9}, 
   3.5418198088954554`*^9, {3.5418639157017345`*^9, 3.5418639366993713`*^9}, {
   3.5424849741692996`*^9, 3.542485018208177*^9}, {3.542998978809877*^9, 
   3.5429990951545315`*^9}, 3.542999448764757*^9, {3.543106567649581*^9, 
   3.543106603262618*^9}}],

Cell[BoxData[
 RowBox[{
  FractionBox["1", "100000"], 
  RowBox[{"(", 
   RowBox[{"4251", "+", 
    RowBox[{"40640", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
    RowBox[{"40767", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
    RowBox[{"4251", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"5", ",", "1"}], "]"}], "+", 
       RowBox[{"a", "[", 
        RowBox[{"5", ",", "5"}], "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}]}], "-", 
    RowBox[{"40767", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
    RowBox[{"40640", " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
   ")"}]}]], "Output",
 CellChangeTimes->{
  3.543106607024833*^9, 3.543319482226179*^9, 3.5433199819857635`*^9, 
   3.543363885216137*^9, 3.54386191663229*^9, 3.545265898982264*^9, 
   3.5452666800674143`*^9, 3.5457690626997056`*^9, 3.547221449153015*^9, 
   3.5472285541303964`*^9, 3.547229433328684*^9, 3.547229889649784*^9, 
   3.547229985166247*^9, {3.5472308266123753`*^9, 3.547230842363276*^9}, 
   3.5472322626155095`*^9, 3.547242325917064*^9, 3.5473094072049274`*^9, 
   3.5474027977032814`*^9, 3.5474032586316447`*^9, 3.5474035316372595`*^9, 
   3.5474915595961714`*^9, 3.54776864483698*^9, 3.547830461970278*^9, 
   3.5478998660699654`*^9, 3.551541467432825*^9, 3.552650643809923*^9, 
   3.5526529865899224`*^9, 3.552828549207838*^9, 3.552932605703529*^9, 
   3.5563180428934493`*^9, 3.556325536063504*^9, 3.5563273396979113`*^9, 
   3.55638565664147*^9, 3.556833710306631*^9, 3.5813374486728306`*^9, 
   3.5880208113396535`*^9, 3.5926615979845695`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"Theta4Cond", "=", 
  RowBox[{
   RowBox[{"Solve", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
      "yhsolve", "\[LeftDoubleBracket]", "6", "\[RightDoubleBracket]"}], 
      "\[Equal]", "0"}], ",", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}], 
   "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}]], "Input",
 CellChangeTimes->{{3.51594676032572*^9, 3.5159468099305573`*^9}, {
   3.5159566741957607`*^9, 3.51595678713122*^9}, {3.5159568457875752`*^9, 
   3.5159568538670373`*^9}, {3.515956895125397*^9, 3.5159569150995398`*^9}, {
   3.5159571238684807`*^9, 3.515957170348139*^9}, {3.515958003729806*^9, 
   3.515958007675031*^9}, {3.5159638958349743`*^9, 3.515963896592017*^9}, {
   3.51596399752379*^9, 3.51596400259408*^9}, {3.515964082366643*^9, 
   3.515964102957821*^9}, {3.5159641483604174`*^9, 3.515964231857193*^9}, {
   3.515964279459916*^9, 3.5159642822650766`*^9}, {3.5159728721103873`*^9, 
   3.5159729359940414`*^9}, {3.515973121749666*^9, 3.5159731262559233`*^9}, 
   3.516711531312767*^9, {3.52072837720152*^9, 3.520728378225579*^9}, {
   3.522511106163158*^9, 3.5225111066501856`*^9}, {3.522511147690533*^9, 
   3.5225111497466507`*^9}, {3.5225118207090273`*^9, 3.522511820941041*^9}, {
   3.5418183907673435`*^9, 3.5418185753739023`*^9}, {3.5418186470990047`*^9, 
   3.5418186530663457`*^9}, {3.541818773371227*^9, 3.541818775619355*^9}, 
   3.5418198088954554`*^9, {3.5418639157017345`*^9, 3.5418639366993713`*^9}, {
   3.5424849741692996`*^9, 3.542485018208177*^9}, {3.542998978809877*^9, 
   3.5429990951545315`*^9}, 3.542999448764757*^9, {3.543106567649581*^9, 
   3.5431066163513665`*^9}}],

Cell[BoxData[
 InterpretationBox[
  TagBox[
   PanelBox[GridBox[{
      {
       StyleBox[
        StyleBox[
         DynamicBox[ToBoxes[
           FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"], 
           StandardForm],
          ImageSizeCache->{309., {3., 9.}}],
         StripOnInput->False,
         DynamicUpdating->True], "Panel",
        StripOnInput->False,
        Background->None]},
      {
       ItemBox[
        TagBox[
         RowBox[{"{", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
           RowBox[{"-", 
            RowBox[{"ArcCos", "[", 
             RowBox[{
              RowBox[{"(", 
               RowBox[{
                RowBox[{"346601034", " ", 
                 RowBox[{"Cos", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"3313541760", " ", 
                 SuperscriptBox[
                  RowBox[{"Cos", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], 
                  "2"], " ", 
                 RowBox[{"Cos", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"\[LeftSkeleton]", "277", "\[RightSkeleton]"}]}], 
               ")"}], "/", 
              RowBox[{"(", 
               RowBox[{"2", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"1661948289", " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}], 
                    "2"]}], "+", 
                  RowBox[{"3313541760", " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"1651609600", " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], 
                    "2"]}], "+", 
                  RowBox[{"\[LeftSkeleton]", "27", "\[RightSkeleton]"}], "+", 
                  
                  RowBox[{"1651609600", " ", 
                   SuperscriptBox[
                    RowBox[{"Cos", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], 
                    "2"]}], "+", 
                  RowBox[{"1651609600", " ", 
                   SuperscriptBox[
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "]"}], 
                    "2"], " ", 
                   SuperscriptBox[
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}], 
                    "2"]}]}], ")"}]}], ")"}]}], "]"}]}]}], "}"}],
         Short[#, 5]& ],
        Background->GrayLevel[1],
        BaseStyle->{Deployed -> False},
        Frame->True,
        FrameStyle->GrayLevel[0, 0.2],
        StripOnInput->False]},
      {
       RowBox[{
        ButtonBox[
         StyleBox[
          StyleBox[
           DynamicBox[ToBoxes[
             FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], 
             StandardForm],
            ImageSizeCache->{50., {1., 9.}}],
           StripOnInput->False,
           DynamicUpdating->True], "Panel",
          StripOnInput->False,
          Background->None],
         Appearance->Automatic,
         ButtonFunction:>OutputSizeLimit`ButtonFunction[
          Identity, 361, 5097980468644476936, 5/2],
         Enabled->True,
         Evaluator->Automatic,
         Method->"Queued"], "\[ThinSpace]", 
        ButtonBox[
         StyleBox[
          StyleBox[
           DynamicBox[ToBoxes[
             FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], 
             StandardForm],
            ImageSizeCache->{59., {1., 9.}}],
           StripOnInput->False,
           DynamicUpdating->True], "Panel",
          StripOnInput->False,
          Background->None],
         Appearance->Automatic,
         ButtonFunction:>OutputSizeLimit`ButtonFunction[
          Identity, 361, 5097980468644476936, 5 2],
         Enabled->True,
         Evaluator->Automatic,
         Method->"Queued"], "\[ThinSpace]", 
        ButtonBox[
         StyleBox[
          StyleBox[
           DynamicBox[ToBoxes[
             FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], 
             StandardForm],
            ImageSizeCache->{43., {1., 9.}}],
           StripOnInput->False,
           DynamicUpdating->True], "Panel",
          StripOnInput->False,
          Background->None],
         Appearance->Automatic,
         ButtonFunction:>OutputSizeLimit`ButtonFunction[
          Identity, 361, 5097980468644476936, Infinity],
         Enabled->True,
         Evaluator->Automatic,
         Method->"Queued"], "\[ThinSpace]", 
        ButtonBox[
         StyleBox[
          StyleBox[
           DynamicBox[ToBoxes[
             FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], 
             StandardForm],
            ImageSizeCache->{73., {1., 9.}}],
           StripOnInput->False,
           DynamicUpdating->True], "Panel",
          StripOnInput->False,
          Background->None],
         Appearance->Automatic,
         ButtonFunction:>FrontEndExecute[{
            FrontEnd`SetOptions[
            FrontEnd`$FrontEnd, 
             FrontEnd`PreferencesSettings -> {"Page" -> "Evaluation"}], 
            FrontEnd`FrontEndToken["PreferencesDialog"]}],
         Evaluator->None,
         Method->"Preemptive"]}]}
     },
     GridBoxAlignment->{
      "Columns" -> {{Left}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
       "RowsIndexed" -> {}},
     GridBoxDividers->{
      "Columns" -> {{False}}, "ColumnsIndexed" -> {}, "Rows" -> {{False}}, 
       "RowsIndexed" -> {}},
     GridBoxItemSize->{
      "Columns" -> {{Automatic}}, "ColumnsIndexed" -> {}, "Rows" -> {{1.}}, 
       "RowsIndexed" -> {}},
     GridBoxSpacings->{"Columns" -> {
         Offset[0.27999999999999997`], {
          Offset[0.5599999999999999]}, 
         Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
         Offset[0.2], {
          Offset[1.2]}, 
         Offset[0.2]}, "RowsIndexed" -> {}}],
    DefaultBaseStyle->{},
    FrameMargins->5],
   Deploy,
   DefaultBaseStyle->{Deployed -> True}],
  Out[361]]], "Output",
 CellChangeTimes->CompressedData["
1:eJxTTMoPSmVkYGBQA+Kan/c7PqS+cnzVMqEyIu2V44ttQjtA9Lz5D9XzgHTY
7PVhiemvHEWOGD3iy3rlmKLg5cgPpE1ec/SzZL9y3Dw5ZvfJ3FeOS/a6Gp4F
0v/Wly4E0RHyNbdAdNqqomcg+t61Xt9zQLpNfVogiBZxk2E+D6QnrE34ehlI
m0xXW/cFSG/wPe2ulPfKUS45qxFEP2nrXgyi43dyzfcF0iYc715fAdLZWTcl
PwNpte/hf0XyXznuYK6QZS975fi4csIWlYpXjk4mWTdVgXQSw/5J1UD66crz
ieuA9NlPjXnK9a8cj02SEFcH0t9C43+A6BPLiopcgPQ110MW8g2vHHee1H17
ex3QHp1Sz9m7XznO0f7oW3rklSMAMCKfRw==
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{
            RowBox[{
             RowBox[{"Transpose", "[", 
              RowBox[{"{", 
               RowBox[{"{", 
                RowBox[{
                 RowBox[{
                  SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], ",", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "6"], "[", "t", "]"}], ",", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", 
                 RowBox[{
                  SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], 
               "}"}], "]"}], "/.", "\[InvisibleSpace]", "Theta6Cond"}], "/.", 
            "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", 
           "Theta4Cond"}], "/.", "\[InvisibleSpace]", "atest"}], "/.", 
         "\[InvisibleSpace]", "Theta1Cond"}], "/.", "\[InvisibleSpace]", 
        "atest"}], "/.", "\[InvisibleSpace]", "Theta3Cond"}], "/.", 
      "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", 
     "Theta2Cond"}], "/.", "\[InvisibleSpace]", "atest"}], "/.", 
   "\[InvisibleSpace]", "Theta5Cond"}], "/.", "\[InvisibleSpace]", 
  "atest"}]], "Input",
 CellChangeTimes->{{3.5418641245189266`*^9, 3.5418641840863085`*^9}, {
   3.541864881071509*^9, 3.5418648957224956`*^9}, {3.541864934696731*^9, 
   3.5418650174467573`*^9}, {3.5424874534626837`*^9, 
   3.5424874804663315`*^9}, {3.54248752516041*^9, 3.542487573723295*^9}, 
   3.542487607185354*^9, {3.5429992460511627`*^9, 3.54299941891605*^9}, {
   3.5431064406733184`*^9, 3.543106467062828*^9}, {3.5431066447189894`*^9, 
   3.5431066469821186`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"-", "0.3496221290968971`"}], "}"}], ",", 
   RowBox[{"{", "0.28342212909689707`", "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"-", "0.4828680187207488`"}], "}"}], ",", 
   RowBox[{"{", "0.20216801872074883`", "}"}], ",", 
   RowBox[{"{", "0.34359999999999996`", "}"}], ",", 
   RowBox[{"{", "0.12910000000000002`", "}"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.541863921130544*^9, 3.5418648680338297`*^9, 3.541864899809703*^9, {
   3.5418649452274055`*^9, 3.5418650207479467`*^9}, 3.5418691448700485`*^9, 
   3.5424874839295373`*^9, {3.54248753091682*^9, 3.542487560837672*^9}, 
   3.5424875985741386`*^9, {3.5429992945479364`*^9, 3.542999315406129*^9}, 
   3.5429993526152573`*^9, {3.5429993958207283`*^9, 3.542999421176179*^9}, 
   3.5429994522929587`*^9, 3.5429997443305454`*^9, 3.5430000143642216`*^9, 
   3.5430107343440065`*^9, 3.5430109155913734`*^9, 3.5430794676911077`*^9, 
   3.543079665497405*^9, 3.5430803253764286`*^9, 3.5431063646719713`*^9, {
   3.5431064477427225`*^9, 3.5431064683108993`*^9}, {3.5431066400327215`*^9, 
   3.543106651082353*^9}, 3.5433195007742395`*^9, 3.543320000830841*^9, 
   3.5433639033507433`*^9, 3.543861941930737*^9, 3.5452659379714355`*^9, 
   3.5452666985714684`*^9, 3.5457690805427265`*^9, 3.5472214671720457`*^9, 
   3.5472285542344027`*^9, 3.5472294536898484`*^9, 3.5472299082638483`*^9, 
   3.5472300038973184`*^9, 3.5472308609603395`*^9, 3.547232281206573*^9, 
   3.547242412316261*^9, 3.5473094255099745`*^9, 3.5474028146712513`*^9, 
   3.5474032755726137`*^9, 3.5474035486272316`*^9, 3.547491576696149*^9, 
   3.5477686718735266`*^9, 3.5478304790632553`*^9, 3.5478998830359364`*^9, 
   3.5515414840468545`*^9, 3.5526506608098955`*^9, 3.5526530035638933`*^9, 
   3.5528285662308116`*^9, 3.5529326227575045`*^9, 3.5563180684299097`*^9, 
   3.5563255612239213`*^9, 3.5563273633362637`*^9, 3.5563856842418966`*^9, 
   3.556833734536018*^9, 3.5813374515869975`*^9, 3.588020813858798*^9, 
   3.592661615880593*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"Theta2a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "/.", "Theta2Cond"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta5a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "/.", "Theta5Cond"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta1a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "/.", "Theta1Cond"}], "/.",
     "Theta2Cond"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta3a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "/.", "Theta3Cond"}], "/.",
     "Theta2Cond"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta4a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "/.", "Theta4Cond"}],
        "/.", "Theta1Cond"}], "/.", "Theta3Cond"}], "/.", "Theta2Cond"}], "/.",
     "Theta5Cond"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta6a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "6"], "[", "t", "]"}], "/.", "Theta6Cond"}],
        "/.", "Theta4Cond"}], "/.", "Theta3Cond"}], "/.", "Theta2Cond"}], "/.",
     "Theta5Cond"}]}], ";"}]}], "Input",
 CellChangeTimes->{{3.515957319602676*^9, 3.515957408978788*^9}, {
   3.5159580291182575`*^9, 3.51595803894882*^9}, {3.515958695734386*^9, 
   3.5159587502905064`*^9}, 3.51595934042026*^9, {3.515963224972603*^9, 
   3.5159632256866436`*^9}, {3.515963471284691*^9, 3.515963496007105*^9}, {
   3.516712231378809*^9, 3.516712233642938*^9}, {3.5418209332226577`*^9, 
   3.5418211947166142`*^9}, {3.541864461123909*^9, 3.541864471158395*^9}, {
   3.5424896843377113`*^9, 3.5424897391578474`*^9}, {3.5429997618649764`*^9, 
   3.542999778635006*^9}, {3.542999811301463*^9, 3.5429999377552853`*^9}, 
   3.543000090074957*^9, 3.5431064990206556`*^9, {3.5431065461153493`*^9, 
   3.5431065467463856`*^9}, {3.543106652797451*^9, 3.54310667182854*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a1\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta1a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, 3.5167122415113883`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a2\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta2a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.516712243727515*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a3\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta3a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.516712245702628*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a4\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta4a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.5167122473267207`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a5\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta5a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.5167122473267207`*^9}, {
   3.541864480284411*^9, 3.541864481891214*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a6\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta6a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{3.5418644937037153`*^9, 3.5418694093973155`*^9}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2matopt_torso.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.515897431926337*^9, {3.5158975074906588`*^9, 3.5158975156601257`*^9}, 
   3.515946262132225*^9, 3.5159464148519597`*^9, 3.5207285400618353`*^9, 
   3.5424799416949244`*^9}],

Cell[BoxData["2"], "Output",
 CellChangeTimes->CompressedData["
1:eJwVz11Ik3EYBfDpEjGG3Vi6SCXLRt0ZFfmB9V9KpTlbkS2QlaM1WGsblpJJ
+eZFGawN+kCxQteoRGK8xAzJ9YWrMNKNOQg3lvVuDX1mtlJrNZKe5+Lwuzhw
4KzXmA5p00QiUQ2m4IG5YfQkMEENFjJlWBJ7UI0w0PcG/Vp5UxZGg4WaI2T3
aOPqL+igYV0qglaqSsqj6DPb0fZSLTBXc6KTXLa+MpehoBS9J89tn/eSaZ+n
p8ko1/ODFMsvpJej5omr+8grkm4HWXbvMU/KOX6BVPT4UmSN/ZLVgt7S5/Ok
ZDYQu45qXC+kNuq140IvWhEP/yQ9Ql7dEDovhNVP0ZXF/jY37V8MDb1GT2R2
fiC3rumqjqBjkd1qsmjWzZMHlt26BNqvC9nIeO1Am+oUsOyMzcNkovWPzIgm
a/0NTTr8PaMSsvXAplLH2Sr01+EC24rTwHT3W0bGzgBLf6ssGUflht92smpb
b5C83W+LkVliXjmBwmK92Is+fOdbmkS7djHnIhr6Ht1bZMQffu4y2ShxOEhL
xqa+OjTpLZ0LoB/3JKQL6Nwn078cE7D6v9b8zFZgvuePXBvPAzt7jJsqRuFa
4EY7mqeYaXKiOXG7cUMHsLWsIleGPpF0JMnJHfbmKtSgj+0s5IBtkd79FnIC
O/gyuP/OCLBqa5aixQPsPybFO+A=
  "]]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Zero Dynamics", "Section",
 CellChangeTimes->{{3.513532015025627*^9, 3.513532018772655*^9}, {
  3.5202040661120033`*^9, 3.520204068054115*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"\[ScriptCapitalQ]red", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", 
       RowBox[{"ndof", "-", "1"}]}], "}"}]}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"d\[ScriptCapitalQ]red", "=", 
   RowBox[{
    SubscriptBox["\[PartialD]", "t"], "\[ScriptCapitalQ]red"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{
  3.5314368956049323`*^9, 3.531501294293047*^9, 3.5315014026682453`*^9, 
   3.5315016982501516`*^9, 3.5315029483474436`*^9, 3.531504631373391*^9, {
   3.5316078845788236`*^9, 3.531607891370212*^9}, {3.538604967154131*^9, 
   3.5386049771377015`*^9}, {3.5452420329211445`*^9, 3.5452420442357693`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]], "Input",
 CellChangeTimes->{
  3.5353950828942404`*^9, {3.5452420525152297`*^9, 3.545242053739298*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{"statesubs", "\[IndentingNewLine]", 
 RowBox[{"Zstatesubs", " ", "=", " ", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["z", "1"], "[", "t", "]"}], "\[Rule]", " ", 
     FormBox[
      RowBox[{"z", "[", "1", "]"}],
      TraditionalForm]}], ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["z", "2"], "[", "t", "]"}], "\[Rule]", " ", 
     FormBox[
      RowBox[{"z", "[", "2", "]"}],
      TraditionalForm]}]}], "}"}]}]}], "Input",
 CellChangeTimes->{{3.5202577106190395`*^9, 3.52025774681911*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "1", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "2", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "3", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "4", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "5", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "6"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "6", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "1"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "7", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "2"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "8", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "3"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "9", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "4"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "10", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "5"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "11", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SuperscriptBox[
      SubscriptBox["\[Theta]", "6"], "\[Prime]",
      MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"x", "[", "12", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.525028856163476*^9, 3.525040921990919*^9, 3.525053941437005*^9, 
   3.525221793844591*^9, 3.525222103139996*^9, 3.525222721478431*^9, 
   3.5271019745910406`*^9, 3.527269680971672*^9, 3.5272773059999733`*^9, 
   3.5272774959795074`*^9, 3.5272781921547546`*^9, 3.5353950860610456`*^9, 
   3.5353966746022425`*^9, 3.5354056985738363`*^9, 3.5357214265994673`*^9, 
   3.535855490687434*^9, 3.5366143859179296`*^9, 3.538597376314006*^9, 
   3.5385980886027465`*^9, 3.5385983788973503`*^9, 3.5386051579330425`*^9, 
   3.5386052081309137`*^9, {3.5452420692611637`*^9, 3.5452420718052855`*^9}, 
   3.545242694510936*^9, 3.545745737406885*^9, 3.5457491911787753`*^9, 
   3.5457493763914003`*^9, 3.54574976417474*^9, 3.545756066157547*^9, 
   3.545759875363958*^9, 3.54576908095875*^9, 3.547221467712076*^9, 
   3.5472285547064295`*^9, 3.5472294557779675`*^9, 3.547229908856882*^9, 
   3.547230004554356*^9, 3.5472308615323725`*^9, 3.5472322825316486`*^9, 
   3.5472424146993713`*^9, 3.5473094260850077`*^9, 3.5474028152202835`*^9, 
   3.5474032760726423`*^9, 3.5474035491662626`*^9, 3.5474915772791824`*^9, 
   3.5477686738726406`*^9, 3.5478304795492835`*^9, 3.547899883546965*^9, 
   3.5515414845148554`*^9, 3.552650661295923*^9, 3.5526530040579214`*^9, 
   3.5528285666888375`*^9, 3.552932623291535*^9, 3.556318069162952*^9, 
   3.5563255620249557`*^9, 3.556327364058305*^9, 3.5563856851179466`*^9, 
   3.556833735317491*^9, 3.581337452934075*^9, 3.5880208149858627`*^9, 
   3.592661616322618*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["z", "1"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"z", "[", "1", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["z", "2"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{"z", "[", "2", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.525028856163476*^9, 3.525040921990919*^9, 3.525053941437005*^9, 
   3.525221793844591*^9, 3.525222103139996*^9, 3.525222721478431*^9, 
   3.5271019745910406`*^9, 3.527269680971672*^9, 3.5272773059999733`*^9, 
   3.5272774959795074`*^9, 3.5272781921547546`*^9, 3.5353950860610456`*^9, 
   3.5353966746022425`*^9, 3.5354056985738363`*^9, 3.5357214265994673`*^9, 
   3.535855490687434*^9, 3.5366143859179296`*^9, 3.538597376314006*^9, 
   3.5385980886027465`*^9, 3.5385983788973503`*^9, 3.5386051579330425`*^9, 
   3.5386052081309137`*^9, {3.5452420692611637`*^9, 3.5452420718052855`*^9}, 
   3.545242694510936*^9, 3.545745737406885*^9, 3.5457491911787753`*^9, 
   3.5457493763914003`*^9, 3.54574976417474*^9, 3.545756066157547*^9, 
   3.545759875363958*^9, 3.54576908095875*^9, 3.547221467712076*^9, 
   3.5472285547064295`*^9, 3.5472294557779675`*^9, 3.547229908856882*^9, 
   3.547230004554356*^9, 3.5472308615323725`*^9, 3.5472322825316486`*^9, 
   3.5472424146993713`*^9, 3.5473094260850077`*^9, 3.5474028152202835`*^9, 
   3.5474032760726423`*^9, 3.5474035491662626`*^9, 3.5474915772791824`*^9, 
   3.5477686738726406`*^9, 3.5478304795492835`*^9, 3.547899883546965*^9, 
   3.5515414845148554`*^9, 3.552650661295923*^9, 3.5526530040579214`*^9, 
   3.5528285666888375`*^9, 3.552932623291535*^9, 3.556318069162952*^9, 
   3.5563255620249557`*^9, 3.556327364058305*^9, 3.5563856851179466`*^9, 
   3.556833735317491*^9, 3.581337452934075*^9, 3.5880208149858627`*^9, 
   3.5926616163246183`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["y", 
   RowBox[{"a", ",", "2"}]], "[", 
  RowBox[{"[", 
   RowBox[{"1", ";;", "5"}], "]"}], "]"}]], "Input",
 CellChangeTimes->{
  3.5386023664514256`*^9, {3.538602435147354*^9, 3.5386024352753615`*^9}, 
   3.545242093071479*^9, 3.5457457153796463`*^9, 3.545755742179242*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "+", 
     FractionBox[
      RowBox[{"320", " ", 
       RowBox[{
        SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "641"]}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "6"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.5386023669494534`*^9, 3.538602436073407*^9, 3.538605157943043*^9, 
   3.5386052081799164`*^9, {3.5452420867131233`*^9, 3.545242093894524*^9}, 
   3.5452426945289373`*^9, 3.5457457374380846`*^9, 3.545749191216778*^9, 
   3.5457493764856043`*^9, 3.54574976418174*^9, 3.545756066167547*^9, 
   3.545759875373958*^9, 3.5457690809637504`*^9, 3.5472214677340775`*^9, 
   3.5472285547674327`*^9, 3.54722945581497*^9, 3.5472299088808837`*^9, 
   3.5472300046083593`*^9, 3.5472308615553737`*^9, 3.54723228255665*^9, 
   3.5472424149923844`*^9, 3.5473094261130095`*^9, 3.547402815276286*^9, 
   3.547403276114645*^9, 3.5474035492192655`*^9, 3.547491577359187*^9, 
   3.547768673911643*^9, 3.5478304795762854`*^9, 3.547899883594968*^9, 
   3.5515414845460553`*^9, 3.5526506613269253`*^9, 3.5526530040849233`*^9, 
   3.552828566707839*^9, 3.5529326233175364`*^9, 3.556318069198954*^9, 
   3.5563255620969596`*^9, 3.5563273641033077`*^9, 3.55638568517595*^9, 
   3.556833735351232*^9, 3.5813374530150795`*^9, 3.5880208152908792`*^9, 
   3.59266161634962*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"c", " ", "=", " ", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[PartialD]", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Flatten", "[", "\[ScriptCapitalQ]red", "]"}], ",", "1"}], 
       "}"}]], 
     RowBox[{"Flatten", "[", 
      SubscriptBox["deltap", "hip"], "]"}]}], "/.", "\[InvisibleSpace]", 
    "constsubs"}], "]"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"H", " ", "=", " ", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[ScriptCapitalQ]red", "]"}], ",", "1"}], 
        "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "[", 
        RowBox[{"[", 
         RowBox[{"1", ";;", "5"}], "]"}], "]"}], "]"}]}], "/.", 
     "\[InvisibleSpace]", "constsubs"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Phi", " ", "=", " ", 
  RowBox[{"Join", "[", 
   RowBox[{
    RowBox[{"{", "c", "}"}], ",", "H"}], "]"}]}]}], "Input",
 CellChangeTimes->{{3.520206779207184*^9, 3.5202069001341004`*^9}, {
   3.520720978686349*^9, 3.5207209931991796`*^9}, 3.5353953053422313`*^9, {
   3.535395336011885*^9, 3.535395342329896*^9}, {3.5353954484364824`*^9, 
   3.5353954662517138`*^9}, {3.535400784446344*^9, 3.5354007868175483`*^9}, {
   3.53589178517136*^9, 3.5358917861394157`*^9}, {3.538598111967083*^9, 
   3.5385981231557226`*^9}, {3.538602440957687*^9, 3.5386024410766935`*^9}, {
   3.538604982076984*^9, 3.538604983771081*^9}, 3.5452421748118925`*^9, 
   3.5457457170644493`*^9, 3.5457557403072386`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"-", 
    FractionBox["81407", "100000"]}], ",", 
   RowBox[{"-", 
    FractionBox["40767", "100000"]}], ",", "0", ",", "0", ",", "0", ",", 
   "0"}], "}"}]], "Output",
 CellChangeTimes->{
  3.525028873022421*^9, 3.525040938692827*^9, 3.525053957869671*^9, 
   3.525221810157209*^9, 3.525222118896285*^9, 3.525222737189448*^9, 
   3.527101986213705*^9, 3.5272696921943145`*^9, 3.5272782008595695`*^9, 
   3.535395306262633*^9, 3.535395346448303*^9, {3.5353954518684883`*^9, 
   3.5353954669693146`*^9}, 3.5353966746490426`*^9, 3.5354056985894365`*^9, 
   3.5357214266150675`*^9, 3.5358554907334366`*^9, 3.5358917867244487`*^9, 
   3.5366143859647293`*^9, 3.53859737638701*^9, 3.538598123577747*^9, 
   3.538598378970354*^9, 3.53860244154072*^9, 3.538605157946043*^9, 
   3.538605208194917*^9, 3.5452420995648384`*^9, 3.545242176346978*^9, 
   3.5452426945439377`*^9, 3.545745737453685*^9, 3.54574919125478*^9, 
   3.545749376593609*^9, 3.5457497641877403`*^9, 3.545756066176548*^9, 
   3.545759875383958*^9, 3.54576908096675*^9, 3.5472214677550793`*^9, 
   3.5472285548124356`*^9, 3.547229455849972*^9, 3.5472299089048853`*^9, 
   3.547230004662362*^9, 3.5472308615853753`*^9, 3.5472322825826516`*^9, 
   3.5472424150573883`*^9, 3.5473094261440115`*^9, 3.5474028153252892`*^9, 
   3.547403276147647*^9, 3.5474035492652683`*^9, 3.5474915775031953`*^9, 
   3.547768673947645*^9, 3.5478304795982866`*^9, 3.54789988363297*^9, 
   3.5515414845928555`*^9, 3.552650661363927*^9, 3.552653004113925*^9, 
   3.55282856673384*^9, 3.5529326233365374`*^9, 3.556318069232956*^9, 
   3.556325562159963*^9, 3.5563273641573105`*^9, 3.5563856852359533`*^9, 
   3.556833735388505*^9, 3.5813374531080847`*^9, 3.588020815328882*^9, 
   3.5926616163666205`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      FractionBox["81407", "100000"]}], ",", 
     RowBox[{"-", 
      FractionBox["40767", "100000"]}], ",", "0", ",", "0", ",", "0", ",", 
     "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "0", ",", "0"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1", ",", "0"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}], ",", "1", ",", 
     FractionBox["320", "641"], ",", "0"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{"1", ",", "1", ",", "1", ",", "0", ",", "0", ",", "0"}], "}"}], 
   ",", 
   RowBox[{"{", 
    RowBox[{"1", ",", "1", ",", "1", ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}], ",", 
     RowBox[{"-", "1"}]}], "}"}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.525028873022421*^9, 3.525040938692827*^9, 3.525053957869671*^9, 
   3.525221810157209*^9, 3.525222118896285*^9, 3.525222737189448*^9, 
   3.527101986213705*^9, 3.5272696921943145`*^9, 3.5272782008595695`*^9, 
   3.535395306262633*^9, 3.535395346448303*^9, {3.5353954518684883`*^9, 
   3.5353954669693146`*^9}, 3.5353966746490426`*^9, 3.5354056985894365`*^9, 
   3.5357214266150675`*^9, 3.5358554907334366`*^9, 3.5358917867244487`*^9, 
   3.5366143859647293`*^9, 3.53859737638701*^9, 3.538598123577747*^9, 
   3.538598378970354*^9, 3.53860244154072*^9, 3.538605157946043*^9, 
   3.538605208194917*^9, 3.5452420995648384`*^9, 3.545242176346978*^9, 
   3.5452426945439377`*^9, 3.545745737453685*^9, 3.54574919125478*^9, 
   3.545749376593609*^9, 3.5457497641877403`*^9, 3.545756066176548*^9, 
   3.545759875383958*^9, 3.54576908096675*^9, 3.5472214677550793`*^9, 
   3.5472285548124356`*^9, 3.547229455849972*^9, 3.5472299089048853`*^9, 
   3.547230004662362*^9, 3.5472308615853753`*^9, 3.5472322825826516`*^9, 
   3.5472424150573883`*^9, 3.5473094261440115`*^9, 3.5474028153252892`*^9, 
   3.547403276147647*^9, 3.5474035492652683`*^9, 3.5474915775031953`*^9, 
   3.547768673947645*^9, 3.5478304795982866`*^9, 3.54789988363297*^9, 
   3.5515414845928555`*^9, 3.552650661363927*^9, 3.552653004113925*^9, 
   3.55282856673384*^9, 3.5529326233365374`*^9, 3.556318069232956*^9, 
   3.556325562159963*^9, 3.5563273641573105`*^9, 3.5563856852359533`*^9, 
   3.556833735388505*^9, 3.5813374531080847`*^9, 3.588020815328882*^9, 
   3.592661616368621*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"fbz", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         RowBox[{
          SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "2"}], "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{"fbz", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
      FractionBox[
       RowBox[{
        RowBox[{
         SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
        RowBox[{"p", "[", "1", "]"}]}], 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}]}]], "Input",
 CellChangeTimes->{{3.5202070301795387`*^9, 3.520207046450469*^9}, {
  3.5207210011976366`*^9, 3.5207210068069577`*^9}, {3.5223568926491346`*^9, 
  3.5223568975475435`*^9}, {3.5224294664548655`*^9, 3.5224294698470592`*^9}, {
  3.5358917784279747`*^9, 3.535891781228134*^9}, {3.5452425130038466`*^9, 
  3.5452425168460636`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["yz", 
   RowBox[{"d", ",", "2"}]], " ", "=", " ", 
  RowBox[{
   RowBox[{"Transpose", "[", 
    RowBox[{"{", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", " ", "2", ",", "6"}], "}"}]}], "]"}], "}"}], "]"}], 
   "/.", "fbz"}]}]], "Input",
 CellChangeTimes->{{3.5202070485075865`*^9, 3.5202070659115825`*^9}, 
   3.5353955628002834`*^9, {3.5385981388896227`*^9, 3.5385981389776278`*^9}, {
   3.538602451956316*^9, 3.538602462378912*^9}, {3.5386052046167126`*^9, 
   3.538605204904729*^9}, 3.5452421164727125`*^9, 3.5457457218536577`*^9, 
   3.5457557381651583`*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "+", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "+", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "+", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "+", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"6", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"6", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             RowBox[{"p", "[", "1", "]"}]}], "+", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.5353955733147016`*^9, 3.5353966746802425`*^9, 3.5354056986050367`*^9, 
   3.5357214266306677`*^9, 3.5358554907614384`*^9, 3.53661438601153*^9, 
   3.538597376725029*^9, 3.538598139590663*^9, 3.538598379033358*^9, {
   3.538602446678014*^9, 3.5386024625999246`*^9}, 3.5386051579540434`*^9, {
   3.538605205234748*^9, 3.5386052082249193`*^9}, 3.545242118254815*^9, 
   3.5452421790131245`*^9, 3.545242523387433*^9, 3.545242694563939*^9, 
   3.545745737484885*^9, 3.545749191296782*^9, 3.545749376695615*^9, 
   3.5457497641997414`*^9, 3.5457560661945486`*^9, 3.545759875393958*^9, 
   3.5457690809767513`*^9, 3.5472214677820807`*^9, 3.547228554998446*^9, 
   3.5472294559009748`*^9, 3.5472299090218916`*^9, 3.5472300047223654`*^9, 
   3.5472308616173773`*^9, 3.5472322826136537`*^9, 3.5472424151933956`*^9, 
   3.547309426180013*^9, 3.547402815432295*^9, 3.547403276185649*^9, 
   3.547403549315271*^9, 3.547491577601201*^9, 3.547768674004648*^9, 
   3.5478304796322885`*^9, 3.547899883670972*^9, 3.5515414846240554`*^9, 
   3.5526506614079294`*^9, 3.552653004146927*^9, 3.5528285667578416`*^9, 
   3.552932623366539*^9, 3.556318069289959*^9, 3.5563255622379675`*^9, 
   3.5563273642163143`*^9, 3.5563856853019557`*^9, 3.556833735443178*^9, 
   3.5813374534631047`*^9, 3.5880208157379055`*^9, 3.5926616163856215`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"PhiI", "=", " ", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["z", "1"], "[", "t", "]"}], "}"}], "}"}], ",", 
      SubscriptBox["yz", 
       RowBox[{"d", ",", "2"}]]}], " ", "]"}]}], "]"}]}]], "Input",
 CellChangeTimes->{{3.5202071326113973`*^9, 3.520207202280382*^9}, {
   3.5202072693732195`*^9, 3.5202072797958155`*^9}, {3.520207350020832*^9, 
   3.5202073515609207`*^9}, {3.5202111360613813`*^9, 3.520211136449404*^9}, 
   3.520211291781288*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{"-", 
       FractionBox["321", "641"]}], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}]}],
      "-", 
     FractionBox[
      RowBox[{"100000", " ", 
       RowBox[{
        SubscriptBox["z", "1"], "[", "t", "]"}]}], "81407"]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "-", 
     RowBox[{
      FractionBox["320", "641"], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}]}],
      "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
     FractionBox[
      RowBox[{"100000", " ", 
       RowBox[{
        SubscriptBox["z", "1"], "[", "t", "]"}]}], "81407"]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "-", 
     RowBox[{
      FractionBox["320", "641"], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}]}],
      "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"6", ",", "5"}], "]"}], "-", 
     RowBox[{
      FractionBox["321", "641"], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}]}],
      "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.525028874781497*^9, 3.525040940356279*^9, 3.52505395960088*^9, 
   3.52522181183866*^9, 3.52522212057463*^9, 3.525222738593982*^9, 
   3.5271019873587704`*^9, 3.527269693357381*^9, 3.5272782016551714`*^9, 
   3.5353955778855095`*^9, 3.5353966756318445`*^9, 3.5354056995410385`*^9, 
   3.535721427582269*^9, 3.535855491729494*^9, 3.5366143869787316`*^9, 
   3.5385973775120745`*^9, 3.5385981516723537`*^9, 3.538598388753914*^9, {
   3.538602454681472*^9, 3.538602465570094*^9}, 3.5386051583720675`*^9, 
   3.53860520893596*^9, {3.5452421227640038`*^9, 3.5452421449891963`*^9}, 
   3.545242181314251*^9, 3.5452425397143383`*^9, 3.545242695694002*^9, 
   3.5457457388108873`*^9, 3.545749192743863*^9, 3.545749378174697*^9, 
   3.5457497656488237`*^9, 3.545756067569421*^9, 3.545759876788015*^9, 
   3.5457690820128107`*^9, 3.547221468855142*^9, 3.5472285555084753`*^9, 
   3.5472294564220047`*^9, 3.5472299100849524`*^9, 3.5472300059094334`*^9, 
   3.54723086271544*^9, 3.5472322836817145`*^9, 3.5472424210795865`*^9, 
   3.547309427318078*^9, 3.5474028165203576`*^9, 3.5474032772607107`*^9, 
   3.5474035504123335`*^9, 3.5474915787152643`*^9, 3.547768677320838*^9, 
   3.5478304807363515`*^9, 3.547899884773035*^9, 3.551541485700457*^9, 
   3.552650662483991*^9, 3.5526530052279882`*^9, 3.5528285678409033`*^9, 
   3.552932624458602*^9, 3.556318070857049*^9, 3.5563255639560547`*^9, 
   3.5563273657884035`*^9, 3.556385686831958*^9, 3.556833737206758*^9, 
   3.5813375308045287`*^9, 3.588020888985095*^9, 3.59266161739668*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"PhidotI", " ", "=", " ", 
  RowBox[{"Simplify", "[", 
   RowBox[{
    RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["z", "2"], "[", "t", "]"}], "}"}], "}"}], ",", 
      RowBox[{
       RowBox[{"D", "[", 
        RowBox[{
         SubscriptBox["yz", 
          RowBox[{"d", ",", "2"}]], ",", 
         RowBox[{
          SubscriptBox["z", "1"], "[", "t", "]"}]}], "]"}], "*", 
       RowBox[{
        SubscriptBox["z", "2"], "[", "t", "]"}]}]}], "]"}]}], 
   "]"}]}]], "Input",
 CellChangeTimes->{{3.520272225237257*^9, 3.520272265975587*^9}, {
   3.5202741201076374`*^9, 3.520274121268704*^9}, {3.5452424446190124`*^9, 
   3.5452424737076344`*^9}, 3.545242547313759*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"-", 
     RowBox[{
      FractionBox["1", 
       RowBox[{"81407", " ", 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]}]], 
      RowBox[{
       SuperscriptBox["\[ExponentialE]", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"p", "[", "1", "]"}], "-", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]], " ", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"100000", " ", 
          SuperscriptBox["\[ExponentialE]", 
           FractionBox[
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "4"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"-", 
                RowBox[{"p", "[", "1", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]]], " ", 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]}], "+", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "3"}], "]"}]}], "-", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Cos", "[", 
           FractionBox[
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"-", 
                RowBox[{"p", "[", "1", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "-", 
         RowBox[{"40767", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "1"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}]}], "+", 
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "3"}], "]"}], " ", 
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
          RowBox[{"Sin", "[", 
           FractionBox[
            RowBox[{
             RowBox[{"a", "[", 
              RowBox[{"2", ",", "2"}], "]"}], " ", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"-", 
                RowBox[{"p", "[", "1", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}], " ", 
       RowBox[{
        SubscriptBox["z", "2"], "[", "t", "]"}]}]}]}], "}"}], ",", 
   RowBox[{"{", 
    FractionBox[
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}]}], "-", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "-", 
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}], " ", 
      RowBox[{
       SubscriptBox["z", "2"], "[", "t", "]"}]}], 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox["1", 
      RowBox[{"81407", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{"100000", " ", 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]}], "-", 
        RowBox[{"40640", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}]}], "-", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{"81407", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "3"}], "]"}]}], "-", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{"40640", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "2"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{"40640", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "4"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "-", 
        RowBox[{"81407", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "2"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "-", 
        RowBox[{"81407", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "4"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}], " ", 
      RowBox[{
       SubscriptBox["z", "2"], "[", "t", "]"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox["1", 
      RowBox[{"641", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{"320", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", 
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "2"}], "]"}]}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "3"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "3"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
        RowBox[{"641", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "3"}], "]"}]}], "-", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "-", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "3"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
        RowBox[{"641", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "2"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "3"}], "]"}]}], "-", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "-", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "2"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "3"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"5", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}], 
      " ", 
      RowBox[{
       SubscriptBox["z", "2"], "[", "t", "]"}]}]}], "}"}], ",", 
   RowBox[{"{", 
    FractionBox[
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}]}], "-", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "-", 
        RowBox[{
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}], " ", 
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}], " ", 
      RowBox[{
       SubscriptBox["z", "2"], "[", "t", "]"}]}], 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     FractionBox["1", 
      RowBox[{"641", " ", 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]}]], 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{"321", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", 
                RowBox[{"a", "[", 
                 RowBox[{"3", ",", "2"}], "]"}]}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "3"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "3"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
        RowBox[{"641", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", 
                RowBox[{"a", "[", 
                 RowBox[{"4", ",", "2"}], "]"}]}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "3"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "3"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"4", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
        RowBox[{"641", " ", 
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"-", 
                RowBox[{"a", "[", 
                 RowBox[{"6", ",", "2"}], "]"}]}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "3"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "1"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "2"}], "]"}]}], "+", 
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "3"}], "]"}], " ", 
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "4"}], "]"}]}]}], ")"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"6", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}], 
      " ", 
      RowBox[{
       SubscriptBox["z", "2"], "[", "t", "]"}]}]}], "}"}]}], "}"}]], "Output",\

 CellChangeTimes->CompressedData["
1:eJxTTMoPSmVkYGCwAeI3islNpc6vHKdfnZJWDaRljH4fbATSW9Y2bb8MpFet
FrsHohs8ArWvAOmow4pHM91fOV4xiV2xG0hX7D14fj+QZlTSSbIOf+Vo94P/
JYhOmsgV6ACko5K5q2+B6Laoj9IRrxwPCf2s64t85bh0nvuBsNhXjjMeMmmG
A2nT5TZuIHqNz+OeKCA9rWHRZBAttDQ4kznrleOF9Ym5INpsQ3c5iDYp+tQJ
opP+lWr9BNIH+O1v/QLSaWm5H0H0vaiXir+B9KcNc03/AWkLluOCDNmvHJWY
bCawAGmWeZV7Tua+crxW0G14Fkj/maq9CEQbWiy7DaIDnBY/B9H/OHYHnQPS
0TbhLOeB9MeTr35dBtIm1p7rvwDplLf3PZTyXjmeaC1vAtFvH8xdAqLnvdJY
4Aukz034/PYKkP5j8FrqM5B+5pb+TyT/lWPVtCY59rJXjlqJ87aqVLxyfLiq
7JYqkJ709OTkaiB9/8a9pHVA+jWLQIFyPdD/v3ZIqAPp/u2XfoLovEdMJS5A
uozX10q+4ZWjUdkfnjvrgPF03SF99m6guG+wf+mRV44Ar7r47Q==
  "]]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"zcond", " ", "=", " ", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
      RowBox[{"PhiI", "[", 
       RowBox[{"[", 
        RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "1", ",", "6"}], "}"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5202579006819105`*^9, 3.5202579420752783`*^9}, {
   3.5353956183051805`*^9, 3.535395622517188*^9}, 3.538598184016204*^9, 
   3.5386050023641443`*^9, {3.545242260588701*^9, 3.5452422795597086`*^9}, 
   3.54574572885807*^9, 3.5457557343723516`*^9}],

Cell[CellGroupData[{

Cell[BoxData["zcond"], "Input",
 CellChangeTimes->{{3.5207002643654747`*^9, 3.520700264954509*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{
      RowBox[{"-", 
       FractionBox["321", "641"]}], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}]}],
      "-", 
     FractionBox[
      RowBox[{"100000", " ", 
       RowBox[{
        SubscriptBox["z", "1"], "[", "t", "]"}]}], "81407"]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "-", 
     RowBox[{
      FractionBox["320", "641"], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"2", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}]}],
      "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
     FractionBox[
      RowBox[{"100000", " ", 
       RowBox[{
        SubscriptBox["z", "1"], "[", "t", "]"}]}], "81407"]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "-", 
     RowBox[{
      FractionBox["320", "641"], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}]}],
      "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "6"], "[", "t", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"6", ",", "5"}], "]"}], "-", 
     RowBox[{
      FractionBox["321", "641"], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SuperscriptBox["\[ExponentialE]", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "4"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"p", "[", "1", "]"}], "-", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]]], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "1"}], "]"}], " ", 
            RowBox[{"Cos", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "3"}], "]"}], " ", 
            RowBox[{"Sin", "[", 
             FractionBox[
              RowBox[{
               RowBox[{"a", "[", 
                RowBox[{"3", ",", "2"}], "]"}], " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"-", 
                  RowBox[{"p", "[", "1", "]"}]}], "+", 
                 RowBox[{
                  SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
              RowBox[{"a", "[", 
               RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}], ")"}]}],
      "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       FractionBox[
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "4"}], "]"}], " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"p", "[", "1", "]"}], "-", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"6", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          FractionBox[
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"6", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               RowBox[{"p", "[", "1", "]"}]}], "+", 
              RowBox[{
               SubscriptBox["z", "1"], "[", "t", "]"}]}], ")"}]}], 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "1"}], "]"}]], "]"}]}]}], ")"}]}]}]}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.525028883331323*^9, 3.525040948919084*^9, 3.525053968189339*^9, 
   3.52522182030486*^9, 3.525222128806129*^9, 3.525222746622542*^9, 
   3.5271019935071225`*^9, 3.5272696990127044`*^9, 3.5272782062571793`*^9, 
   3.5353956255435934`*^9, 3.535396680577053*^9, 3.535405704345847*^9, 
   3.5357214324182777`*^9, 3.5358554990839148`*^9, 3.53661439184594*^9, 
   3.53859831630077*^9, 3.538598449927413*^9, 3.5386051591491117`*^9, 
   3.538605213289209*^9, {3.54524226731308*^9, 3.5452422834219294`*^9}, 
   3.5452425589734073`*^9, 3.545242703623443*^9, 3.545745747952503*^9, 
   3.5457492022206745`*^9, 3.545749387703766*^9, 3.5457497754833527`*^9, 
   3.545756076958851*^9, 3.545759886246267*^9, 3.545769089978266*^9, 
   3.5472214759305468`*^9, 3.5472285565795364`*^9, 3.547229457432062*^9, 
   3.5472299173033657`*^9, 3.547230013169849*^9, 3.5472308698828497`*^9, 
   3.5472322909211287`*^9, 3.547242455801176*^9, 3.547309434357481*^9, 
   3.547402823104734*^9, 3.547403283776083*^9, 3.5474035570217113`*^9, 
   3.547491585400647*^9, 3.5477686877174325`*^9, 3.547830487500738*^9, 
   3.5478998912964087`*^9, 3.551541492158869*^9, 3.5526506690193653`*^9, 
   3.5526530117463617`*^9, 3.552828574411279*^9, 3.5529326309999757`*^9, 
   3.556318080584605*^9, 3.556325573943626*^9, 3.5563273746669116`*^9, 
   3.5563856962901406`*^9, 3.556833746518291*^9, 3.5813377041184416`*^9, 
   3.5880210502443185`*^9, 3.592661626705212*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Phi1_inverse_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"PhiI", "/.", "Zstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202568164348955`*^9, 3.5202568378191185`*^9}, 
   3.5202577668162537`*^9, 3.545242314540693*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Phi1_dot_inverse_mat\>\"", "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"PhidotI", "/.", "Zstatesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5202723515834837`*^9, 3.5202723547236633`*^9}, 
   3.5452423157797637`*^9}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2matopt_torso.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.515897431926337*^9, {3.5158975074906588`*^9, 3.5158975156601257`*^9}, 
   3.515946262132225*^9, 3.5159464148519597`*^9, {3.535396571978861*^9, 
   3.535396574272065*^9}, {3.545242347920573*^9, 3.545242358756178*^9}, {
   3.54574921912574*^9, 3.5457492373154955`*^9}}],

Cell[BoxData["2"], "Output",
 CellChangeTimes->{
  3.525028889439977*^9, 3.525040955239027*^9, 3.525053974252254*^9, 
   3.525221826450718*^9, 3.525222134367461*^9, 3.525222752002709*^9, 
   3.5271019982443933`*^9, 3.5272697032789483`*^9, 3.527278209470785*^9, 
   3.535396680733053*^9, 3.5354057044238467`*^9, 3.5357214325274777`*^9, 
   3.5358554993609304`*^9, 3.53661439195514*^9, 3.5385984499774156`*^9, 
   3.5386051592241163`*^9, 3.5386052134172163`*^9, {3.5452425644847093`*^9, 
   3.545242571426098*^9}, 3.545242625793116*^9, 3.5452427037554507`*^9, 
   3.5457457480929036`*^9, {3.545749202390675*^9, 3.5457492381695414`*^9}, 
   3.545749387959779*^9, 3.5457497756113605`*^9, 3.545756077179864*^9, 
   3.545759886385275*^9, 3.545769090316285*^9, 3.547221476145559*^9, 
   3.5472285566815424`*^9, 3.5472294578710876`*^9, 3.5472299174583745`*^9, 
   3.547230013347859*^9, 3.5472308700298586`*^9, 3.5472322911261406`*^9, 
   3.5472424562212*^9, 3.5473094345464916`*^9, 3.5474028232357416`*^9, 
   3.5474032838940897`*^9, 3.5474035578767605`*^9, 3.547491585559656*^9, 
   3.5477686882154613`*^9, 3.547830487663748*^9, 3.547899891411415*^9, 
   3.551541492268069*^9, 3.5526506691433725`*^9, 3.5526530118573675`*^9, 
   3.5528285745392866`*^9, 3.5529326311029816`*^9, 3.5563180808066177`*^9, 
   3.556325574167639*^9, 3.5563273748749237`*^9, 3.556385696530141*^9, 
   3.556833746697216*^9, 3.5813377043624554`*^9, 3.5880210504533305`*^9, 
   3.5926616268042173`*^9}]
}, Open  ]],

Cell[BoxData[" "], "Input",
 CellChangeTimes->{3.547491528039366*^9}]
}, Closed]]
}, Open  ]]
},
WindowSize->{1264, 1342},
WindowMargins->{{Automatic, 0}, {Automatic, 0}},
PrivateNotebookOptions->{"VersionedStylesheet"->{"Default.nb"[8.] -> False}},
FrontEndVersion->"10.0 for Microsoft Windows (64-bit) (July 1, 2014)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[567, 22, 336, 5, 101, "Title"],
Cell[CellGroupData[{
Cell[928, 31, 196, 3, 70, "Section"],
Cell[CellGroupData[{
Cell[1149, 38, 1802, 50, 232, "Input"],
Cell[2954, 90, 249, 3, 31, "Output"],
Cell[3206, 95, 568, 13, 21, "Message"],
Cell[3777, 110, 825, 16, 21, "Message"],
Cell[4605, 128, 810, 16, 21, "Message"],
Cell[5418, 146, 827, 16, 21, "Message"],
Cell[6248, 164, 818, 16, 21, "Message"],
Cell[7069, 182, 815, 16, 21, "Message"],
Cell[7887, 200, 801, 16, 21, "Message"],
Cell[8691, 218, 812, 16, 21, "Message"],
Cell[9506, 236, 809, 16, 21, "Message"],
Cell[10318, 254, 554, 11, 21, "Message"],
Cell[10875, 267, 104, 1, 31, "Output"],
Cell[10982, 270, 454, 8, 21, "Message"],
Cell[11439, 280, 101, 1, 31, "Output"],
Cell[11543, 283, 605, 11, 21, "Message"]
}, Open  ]],
Cell[12163, 297, 226, 5, 31, "Input"],
Cell[12392, 304, 1558, 38, 72, "Input"],
Cell[13953, 344, 94, 1, 31, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[14084, 350, 207, 3, 70, "Section"],
Cell[CellGroupData[{
Cell[14316, 357, 155, 2, 29, "Item"],
Cell[14474, 361, 103, 1, 29, "Item"],
Cell[14580, 364, 374, 8, 29, "Item"],
Cell[14957, 374, 128, 2, 29, "Item"],
Cell[15088, 378, 104, 1, 29, "Item"],
Cell[15195, 381, 197, 2, 29, "Item"],
Cell[15395, 385, 196, 2, 29, "Item"],
Cell[15594, 389, 378, 5, 29, "Item"],
Cell[15975, 396, 171, 2, 29, "Item"],
Cell[16149, 400, 169, 2, 29, "Item"],
Cell[16321, 404, 196, 2, 29, "Item"]
}, Open  ]],
Cell[16532, 409, 5104, 84, 132, "Input"],
Cell[21639, 495, 1521, 21, 31, "Input"],
Cell[CellGroupData[{
Cell[23185, 520, 922, 19, 46, "Input"],
Cell[24110, 541, 4147, 77, 82, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[28294, 623, 352, 6, 31, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[28649, 631, 2940, 57, 52, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}]
}, Open  ]],
Cell[31604, 691, 1724, 29, 46, "Output"],
Cell[33331, 722, 3416, 82, 52, "Output"],
Cell[36750, 806, 2278, 65, 52, "Input"],
Cell[39031, 873, 5514, 147, 423, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[44548, 1022, 110, 1, 30, "Text"],
Cell[44661, 1025, 740, 21, 36, "Input"],
Cell[CellGroupData[{
Cell[45426, 1050, 783, 23, 46, "Input"],
Cell[46212, 1075, 1087, 28, 103, "Output"]
}, Open  ]],
Cell[47314, 1106, 2176, 57, 52, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[CellGroupData[{
Cell[49515, 1167, 215, 4, 31, "Input"],
Cell[49733, 1173, 1117, 28, 46, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[50887, 1206, 180, 3, 31, "Input"],
Cell[51070, 1211, 1496, 40, 82, "Output"]
}, Open  ]],
Cell[52581, 1254, 92, 1, 31, "Input"],
Cell[52676, 1257, 111, 1, 30, "Text"],
Cell[CellGroupData[{
Cell[52812, 1262, 2835, 70, 76, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[55650, 1334, 1454, 37, 31, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}]
}, Open  ]],
Cell[CellGroupData[{
Cell[57141, 1376, 135, 2, 49, "Subsection"],
Cell[57279, 1380, 1169, 28, 72, "Input"],
Cell[58451, 1410, 1010, 26, 52, "Input"],
Cell[CellGroupData[{
Cell[59486, 1440, 148, 2, 39, "Subsubsection"],
Cell[59637, 1444, 202, 3, 30, "Text"],
Cell[CellGroupData[{
Cell[59864, 1451, 6882, 163, 212, "Input"],
Cell[66749, 1616, 1814, 27, 31, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[68612, 1649, 134, 2, 39, "Subsubsection"],
Cell[68749, 1653, 4522, 123, 187, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[73308, 1781, 108, 1, 39, "Subsubsection"],
Cell[73419, 1784, 8861, 248, 187, "Input"],
Cell[82283, 2034, 11849, 354, 326, "Input"],
Cell[CellGroupData[{
Cell[94157, 2392, 15256, 455, 384, "Input"],
Cell[109416, 2849, 1102, 17, 31, "Output"]
}, Open  ]],
Cell[110533, 2869, 2014, 55, 120, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[112584, 2929, 265, 4, 39, "Subsubsection"],
Cell[112852, 2935, 7032, 181, 272, "Input"],
Cell[119887, 3118, 11466, 342, 272, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[131390, 3465, 145, 2, 39, "Subsubsection"],
Cell[CellGroupData[{
Cell[131560, 3471, 2538, 54, 31, "Input"],
Cell[134101, 3527, 23513, 636, 653, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[157651, 4168, 272, 6, 31, "Input"],
Cell[157926, 4176, 7200, 187, 88, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[165163, 4368, 1248, 36, 31, "Input"],
Cell[166414, 4406, 3032, 60, 31, "Output"]
}, Open  ]],
Cell[169461, 4469, 2440, 51, 112, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[171938, 4525, 184, 3, 39, "Subsubsection"],
Cell[172125, 4530, 1262, 29, 58, "Input"],
Cell[CellGroupData[{
Cell[173412, 4563, 344, 7, 31, "Input"],
Cell[173759, 4572, 4599, 88, 192, "Output"]
}, Open  ]],
Cell[178373, 4663, 911, 17, 31, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[179321, 4685, 142, 2, 39, "Subsubsection"],
Cell[CellGroupData[{
Cell[179488, 4691, 991, 18, 31, "Input"],
Cell[180482, 4711, 406, 10, 21, "Message"],
Cell[180891, 4723, 408, 10, 21, "Message"],
Cell[181302, 4735, 77, 1, 31, "Output"]
}, Open  ]],
Cell[181394, 4739, 453, 10, 31, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[181884, 4754, 247, 4, 39, "Subsubsection"],
Cell[CellGroupData[{
Cell[182156, 4762, 2310, 50, 102, "Input"],
Cell[184469, 4814, 596, 12, 23, "Message"],
Cell[185068, 4828, 596, 12, 23, "Message"],
Cell[185667, 4842, 596, 12, 23, "Message"],
Cell[186266, 4856, 528, 11, 21, "Message"],
Cell[186797, 4869, 452, 9, 21, "Message"],
Cell[187252, 4880, 449, 9, 21, "Message"],
Cell[187704, 4891, 452, 9, 21, "Message"],
Cell[188159, 4902, 532, 11, 21, "Message"],
Cell[188694, 4915, 280, 8, 21, "Message"],
Cell[188977, 4925, 282, 8, 21, "Message"],
Cell[189262, 4935, 280, 8, 21, "Message"],
Cell[189545, 4945, 282, 8, 21, "Message"],
Cell[189830, 4955, 280, 8, 21, "Message"],
Cell[190113, 4965, 278, 8, 21, "Message"],
Cell[190394, 4975, 280, 8, 21, "Message"],
Cell[190677, 4985, 282, 8, 21, "Message"],
Cell[190962, 4995, 280, 8, 21, "Message"],
Cell[191245, 5005, 282, 8, 21, "Message"],
Cell[191530, 5015, 279, 8, 21, "Message"],
Cell[191812, 5025, 280, 8, 21, "Message"],
Cell[192095, 5035, 344, 9, 21, "Message"],
Cell[192442, 5046, 342, 9, 21, "Message"],
Cell[192787, 5057, 344, 9, 21, "Message"],
Cell[193134, 5068, 344, 9, 21, "Message"],
Cell[193481, 5079, 282, 8, 21, "Message"],
Cell[193766, 5089, 282, 8, 21, "Message"],
Cell[194051, 5099, 280, 8, 21, "Message"],
Cell[194334, 5109, 282, 8, 21, "Message"],
Cell[194619, 5119, 282, 8, 21, "Message"],
Cell[194904, 5129, 282, 8, 21, "Message"],
Cell[195189, 5139, 280, 8, 21, "Message"],
Cell[195472, 5149, 280, 8, 21, "Message"],
Cell[195755, 5159, 280, 8, 21, "Message"],
Cell[196038, 5169, 282, 8, 21, "Message"],
Cell[196323, 5179, 282, 8, 21, "Message"],
Cell[196608, 5189, 282, 8, 21, "Message"],
Cell[196893, 5199, 344, 9, 21, "Message"],
Cell[197240, 5210, 344, 9, 21, "Message"],
Cell[197587, 5221, 344, 9, 21, "Message"],
Cell[197934, 5232, 342, 9, 21, "Message"],
Cell[198279, 5243, 1431, 36, 31, "Output"]
}, Open  ]],
Cell[199725, 5282, 1459, 38, 76, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[201221, 5325, 123, 1, 39, "Subsubsection"],
Cell[201347, 5328, 515, 12, 31, "Input"],
Cell[201865, 5342, 2467, 60, 92, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[204335, 5404, 3134, 88, 92, "Input"],
Cell[CellGroupData[{
Cell[207494, 5496, 1562, 39, 72, "Input"],
Cell[209059, 5537, 1074, 19, 40, "Message"],
Cell[210136, 5558, 1076, 19, 40, "Message"],
Cell[211215, 5579, 1896, 31, 75, "Message"],
Cell[213114, 5612, 1095, 19, 57, "Message"],
Cell[214212, 5633, 1095, 19, 57, "Message"],
Cell[215310, 5654, 530, 11, 21, "Message"]
}, Open  ]],
Cell[CellGroupData[{
Cell[215877, 5670, 1610, 41, 72, "Input"],
Cell[217490, 5713, 1963, 31, 120, "Message"],
Cell[219456, 5746, 1963, 31, 120, "Message"],
Cell[221422, 5779, 2055, 32, 120, "Message"],
Cell[223480, 5813, 1982, 31, 120, "Message"],
Cell[225465, 5846, 1984, 31, 120, "Message"],
Cell[227452, 5879, 528, 11, 21, "Message"]
}, Open  ]],
Cell[227995, 5893, 2195, 45, 31, "Input"],
Cell[CellGroupData[{
Cell[230215, 5942, 590, 15, 31, "Input"],
Cell[230808, 5959, 1095, 19, 57, "Message"],
Cell[231906, 5980, 452, 9, 21, "Message"],
Cell[232361, 5991, 452, 9, 21, "Message"],
Cell[232816, 6002, 18984, 432, 179, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[251837, 6439, 1401, 34, 31, "Input"],
Cell[253241, 6475, 1076, 19, 40, "Message"],
Cell[254320, 6496, 1965, 31, 120, "Message"],
Cell[256288, 6529, 1945, 31, 120, "Message"],
Cell[258236, 6562, 458, 10, 21, "Message"],
Cell[258697, 6574, 458, 10, 21, "Message"],
Cell[259158, 6586, 530, 11, 21, "Message"]
}, Open  ]],
Cell[CellGroupData[{
Cell[259725, 6602, 203, 4, 31, "Input"],
Cell[259931, 6608, 1632, 23, 31, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[261600, 6636, 862, 17, 31, "Input"],
Cell[262465, 6655, 460, 10, 21, "Message"],
Cell[262928, 6667, 458, 10, 21, "Message"],
Cell[263389, 6679, 460, 10, 21, "Message"],
Cell[263852, 6691, 532, 11, 21, "Message"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[264433, 6708, 306, 5, 39, "Subsubsection"],
Cell[264742, 6715, 1273, 28, 52, "Input"],
Cell[CellGroupData[{
Cell[266040, 6747, 116, 3, 31, "Input"],
Cell[266159, 6752, 16343, 438, 558, "Output"]
}, Open  ]],
Cell[282517, 7193, 1207, 33, 192, "Input"],
Cell[283727, 7228, 941, 20, 92, "Input"],
Cell[284671, 7250, 842, 19, 92, "Input"],
Cell[285516, 7271, 842, 19, 92, "Input"],
Cell[286361, 7292, 577, 15, 92, "Input"],
Cell[286941, 7309, 993, 21, 92, "Input"],
Cell[287937, 7332, 524, 14, 92, "Input"],
Cell[288464, 7348, 1257, 25, 92, "Input"],
Cell[289724, 7375, 623, 18, 95, "Input"],
Cell[290350, 7395, 1103, 23, 92, "Input"],
Cell[291456, 7420, 1157, 23, 92, "Input"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[292674, 7450, 158, 2, 71, "Section"],
Cell[292835, 7454, 157, 2, 29, "Text"],
Cell[292995, 7458, 1344, 31, 52, "Input"],
Cell[CellGroupData[{
Cell[294364, 7493, 101, 1, 36, "Subsection"],
Cell[CellGroupData[{
Cell[294490, 7498, 132, 2, 27, "Subsubsection"],
Cell[CellGroupData[{
Cell[294647, 7504, 588, 11, 55, "Input"],
Cell[295238, 7517, 1599, 27, 70, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[296886, 7550, 82, 1, 27, "Subsubsection"],
Cell[CellGroupData[{
Cell[296993, 7555, 553, 11, 31, "Input"],
Cell[297549, 7568, 1842, 29, 70, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[299440, 7603, 121, 1, 27, "Subsubsection"],
Cell[299564, 7606, 418, 7, 31, "Input"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[300031, 7619, 199, 3, 36, "Subsection"],
Cell[CellGroupData[{
Cell[300255, 7626, 159, 2, 27, "Subsubsection"],
Cell[300417, 7630, 1571, 37, 53, "Input"],
Cell[CellGroupData[{
Cell[302013, 7671, 244, 5, 31, "Input"],
Cell[302260, 7678, 2881, 62, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[305178, 7745, 224, 6, 31, "Input"],
Cell[305405, 7753, 1977, 38, 70, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[307431, 7797, 170, 2, 27, "Subsubsection"],
Cell[CellGroupData[{
Cell[307626, 7803, 1243, 24, 52, "Input"],
Cell[308872, 7829, 2752, 47, 70, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[311685, 7883, 175, 2, 36, "Subsection"],
Cell[311863, 7887, 600, 11, 31, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[312500, 7903, 176, 2, 36, "Subsection"],
Cell[312679, 7907, 117, 1, 29, "Text"],
Cell[312799, 7910, 557, 10, 31, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[313393, 7925, 242, 3, 36, "Subsection"],
Cell[CellGroupData[{
Cell[313660, 7932, 834, 16, 31, "Input"],
Cell[314497, 7950, 2097, 37, 70, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[316643, 7993, 102, 1, 36, "Subsection"],
Cell[CellGroupData[{
Cell[316770, 7998, 1845, 34, 31, "Input"],
Cell[318618, 8034, 2692, 48, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[321347, 8087, 113, 1, 27, "Subsubsection"],
Cell[CellGroupData[{
Cell[321485, 8092, 618, 15, 31, "Input"],
Cell[322106, 8109, 2244, 39, 70, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[324411, 8155, 131, 2, 36, "Subsection"],
Cell[CellGroupData[{
Cell[324567, 8161, 962, 16, 31, "Input"],
Cell[325532, 8179, 3482, 70, 70, "Output"]
}, Open  ]]
}, Open  ]]
}, Closed]],
Cell[CellGroupData[{
Cell[329075, 8256, 101, 1, 41, "Section"],
Cell[329179, 8259, 185, 2, 29, "Text"],
Cell[329367, 8263, 526, 12, 52, "Input"],
Cell[329896, 8277, 742, 19, 52, "Input"],
Cell[330641, 8298, 1410, 34, 77, "Input"],
Cell[332054, 8334, 163, 2, 29, "Text"],
Cell[CellGroupData[{
Cell[332242, 8340, 1092, 21, 50, "Input"],
Cell[333337, 8363, 2321, 45, 57, "Output"]
}, Open  ]],
Cell[335673, 8411, 307, 7, 31, "Input"],
Cell[CellGroupData[{
Cell[336005, 8422, 263, 8, 27, "Subsubsection"],
Cell[336271, 8432, 986, 30, 49, "Input"],
Cell[CellGroupData[{
Cell[337282, 8466, 1212, 28, 52, "Input"],
Cell[338497, 8496, 15455, 422, 329, "Output"]
}, Open  ]],
Cell[353967, 8921, 1387, 35, 55, "Input"],
Cell[355357, 8958, 1113, 33, 55, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[356507, 8996, 189, 2, 27, "Subsubsection"],
Cell[CellGroupData[{
Cell[356721, 9002, 2018, 36, 52, "Input"],
Cell[358742, 9040, 2333, 42, 47, "Output"],
Cell[361078, 9084, 3330, 73, 47, "Output"]
}, Open  ]],
Cell[364423, 9160, 1278, 34, 55, "Input"],
Cell[365704, 9196, 1601, 40, 55, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[367342, 9241, 122, 1, 27, "Subsubsection"],
Cell[367467, 9244, 884, 19, 92, "Input"],
Cell[368354, 9265, 978, 21, 92, "Input"],
Cell[369335, 9288, 1098, 23, 92, "Input"],
Cell[370436, 9313, 1014, 22, 95, "Input"],
Cell[371453, 9337, 1137, 24, 95, "Input"],
Cell[372593, 9363, 915, 20, 92, "Input"],
Cell[373511, 9385, 637, 17, 92, "Input"],
Cell[374151, 9404, 834, 20, 92, "Input"],
Cell[374988, 9426, 642, 17, 95, "Input"],
Cell[375633, 9445, 783, 19, 95, "Input"],
Cell[376419, 9466, 692, 18, 95, "Input"],
Cell[377114, 9486, 696, 18, 95, "Input"],
Cell[377813, 9506, 637, 17, 92, "Input"],
Cell[378453, 9525, 740, 19, 92, "Input"],
Cell[379196, 9546, 639, 17, 95, "Input"],
Cell[379838, 9565, 741, 19, 95, "Input"],
Cell[380582, 9586, 812, 20, 95, "Input"],
Cell[381397, 9608, 642, 17, 95, "Input"]
}, Open  ]],
Cell[CellGroupData[{
Cell[382076, 9630, 91, 1, 36, "Subsection"],
Cell[382170, 9633, 130, 1, 29, "Text"],
Cell[CellGroupData[{
Cell[382325, 9638, 1220, 23, 72, "Input"],
Cell[383548, 9663, 2343, 33, 30, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[385952, 9703, 251, 3, 71, "Section"],
Cell[386206, 9708, 270, 7, 31, "Input"],
Cell[386479, 9717, 1461, 30, 132, "Input"],
Cell[387943, 9749, 302, 7, 31, "Input"],
Cell[CellGroupData[{
Cell[388270, 9760, 796, 22, 92, "Input"],
Cell[389069, 9784, 2203, 48, 30, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[391309, 9837, 260, 3, 36, "Subsection",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[391572, 9842, 999, 20, 33, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[392574, 9864, 408, 7, 31, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[392985, 9873, 302, 8, 31, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[393290, 9883, 913, 20, 52, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}],
Cell[394206, 9905, 523, 14, 92, "Input",
 CellGroupingRules->{GroupTogetherGrouping, 10000.}]
}, Open  ]],
Cell[394744, 9922, 137, 1, 29, "Text"],
Cell[394884, 9925, 700, 17, 31, "Input"],
Cell[395587, 9944, 136, 1, 29, "Text"],
Cell[CellGroupData[{
Cell[395748, 9949, 360, 9, 31, "Input"],
Cell[396111, 9960, 4722, 106, 73, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[400870, 10071, 551, 17, 31, "Input"],
Cell[401424, 10090, 3311, 79, 47, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[404772, 10174, 482, 10, 31, "Input"],
Cell[405257, 10186, 2066, 40, 30, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[407360, 10231, 359, 8, 31, "Input"],
Cell[407722, 10241, 2049, 45, 30, "Output"]
}, Open  ]],
Cell[409786, 10289, 472, 13, 92, "Input"],
Cell[410261, 10304, 422, 8, 31, "Input"],
Cell[410686, 10314, 477, 13, 112, "Input"],
Cell[CellGroupData[{
Cell[411188, 10331, 331, 4, 36, "Subsection"],
Cell[411522, 10337, 126, 1, 29, "Text"],
Cell[411651, 10340, 957, 24, 33, "Input"],
Cell[CellGroupData[{
Cell[412633, 10368, 301, 7, 31, "Input"],
Cell[412937, 10377, 9397, 257, 201, "Output"]
}, Open  ]],
Cell[422349, 10637, 489, 14, 92, "Input"],
Cell[CellGroupData[{
Cell[422863, 10655, 438, 8, 31, "Input"],
Cell[423304, 10665, 48910, 1298, 919, "Output"]
}, Open  ]],
Cell[472229, 11966, 451, 12, 92, "Input"],
Cell[CellGroupData[{
Cell[472705, 11982, 375, 9, 52, "Input"],
Cell[473083, 11993, 3660, 80, 47, "Output"],
Cell[476746, 12075, 10568, 269, 191, "Output"]
}, Open  ]],
Cell[487329, 12347, 803, 18, 92, "Input"],
Cell[CellGroupData[{
Cell[488157, 12369, 327, 4, 27, "Subsubsection"],
Cell[488487, 12375, 131, 1, 29, "Text"],
Cell[488621, 12378, 2315, 59, 192, "Input"],
Cell[490939, 12439, 3697, 105, 72, "Input"],
Cell[494639, 12546, 549, 14, 31, "Input"],
Cell[CellGroupData[{
Cell[495213, 12564, 124, 2, 31, "Input"],
Cell[495340, 12568, 2444, 46, 50, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[497821, 12619, 101, 1, 31, "Input"],
Cell[497925, 12622, 3551, 90, 73, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[501513, 12717, 198, 5, 31, "Input"],
Cell[501714, 12724, 1383, 25, 30, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[503134, 12754, 200, 5, 31, "Input"],
Cell[503337, 12761, 1382, 25, 30, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[504756, 12791, 541, 11, 31, "Input"],
Cell[505300, 12804, 1839, 35, 47, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[507176, 12844, 247, 6, 31, "Input"],
Cell[507426, 12852, 1549, 30, 30, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[509012, 12887, 320, 8, 31, "Input"],
Cell[509335, 12897, 1649, 34, 47, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[511021, 12936, 143, 3, 31, "Input"],
Cell[511167, 12941, 3119, 72, 47, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[514323, 13018, 895, 15, 52, "Input"],
Cell[515221, 13035, 5975, 152, 149, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[521233, 13192, 3474, 84, 132, "Input"],
Cell[524710, 13278, 2746, 67, 47, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[527493, 13350, 1678, 29, 31, "Input"],
Cell[529174, 13381, 10385, 266, 164, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[539596, 13652, 1954, 44, 52, "Input"],
Cell[541553, 13698, 2031, 33, 30, "Output"]
}, Open  ]],
Cell[543599, 13734, 2246, 58, 132, "Input"],
Cell[545848, 13794, 608, 15, 92, "Input"],
Cell[546459, 13811, 632, 15, 92, "Input"],
Cell[547094, 13828, 632, 15, 92, "Input"],
Cell[547729, 13845, 634, 15, 92, "Input"],
Cell[548366, 13862, 684, 16, 92, "Input"],
Cell[549053, 13880, 453, 12, 92, "Input"],
Cell[CellGroupData[{
Cell[549531, 13896, 592, 15, 72, "Input"],
Cell[550126, 13913, 771, 14, 30, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[550970, 13935, 149, 2, 71, "Section"],
Cell[551122, 13939, 819, 21, 52, "Input"],
Cell[551944, 13962, 269, 7, 31, "Input"],
Cell[CellGroupData[{
Cell[552238, 13973, 546, 16, 52, "Input"],
Cell[552787, 13991, 3551, 84, 70, "Output"],
Cell[556341, 14077, 1820, 32, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[558198, 14114, 315, 8, 31, "Input"],
Cell[558516, 14124, 2557, 63, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[561110, 14192, 1667, 42, 74, "Input"],
Cell[562780, 14236, 1790, 30, 70, "Output"],
Cell[564573, 14268, 2498, 52, 70, "Output"]
}, Open  ]],
Cell[567086, 14323, 1170, 33, 78, "Input"],
Cell[CellGroupData[{
Cell[568281, 14360, 703, 17, 33, "Input"],
Cell[568987, 14379, 10151, 287, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[579175, 14671, 639, 16, 33, "Input"],
Cell[579817, 14689, 21298, 606, 70, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[601152, 15300, 813, 23, 33, "Input"],
Cell[601968, 15325, 31945, 893, 70, "Output"]
}, Open  ]],
Cell[633928, 16221, 661, 16, 31, "Input"],
Cell[CellGroupData[{
Cell[634614, 16241, 99, 1, 31, "Input"],
Cell[634716, 16244, 21604, 617, 70, "Output"]
}, Open  ]],
Cell[656335, 16864, 511, 13, 92, "Input"],
Cell[656849, 16879, 499, 14, 92, "Input"],
Cell[CellGroupData[{
Cell[657373, 16897, 687, 16, 72, "Input"],
Cell[658063, 16915, 1468, 21, 70, "Output"]
}, Open  ]],
Cell[659546, 16939, 69, 1, 31, "Input"]
}, Closed]]
}, Open  ]]
}
]
*)

(* End of internal cache information *)
